(* Content-type: application/mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 7.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       145,          7]
NotebookDataLength[    827282,      20750]
NotebookOptionsPosition[    808085,      20159]
NotebookOutlinePosition[    808641,      20182]
CellTagsIndexPosition[    808553,      20177]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{

Cell[CellGroupData[{
Cell["2d kneed biped with torso", "Title",
 CellChangeTimes->{
  3.504390813710744*^9, {3.513011450427533*^9, 3.51301145527552*^9}, {
   3.513011745947039*^9, 3.513011749321201*^9}, {3.515858432663828*^9, 
   3.5158584360910234`*^9}, {3.5158700644438047`*^9, 
   3.5158700646998196`*^9}, {3.5158859650463057`*^9, 3.515885965342706*^9}}],

Cell[CellGroupData[{

Cell["initialization", "Section",
 CellChangeTimes->{
  3.504390830432091*^9, {3.513011606627004*^9, 3.513011611923624*^9}, {
   3.513011744096037*^9, 3.513011744363876*^9}, 3.514056528643134*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], ";"}], "\n", 
 RowBox[{"<<", "RobotLinks.m"}], "\n", 
 RowBox[{"<<", "ToMatlabMod.m"}], "\[IndentingNewLine]", 
 RowBox[{"<<", "Linearize.m"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build_torso\>\""}], 
   "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"t0", "=", 
   RowBox[{"AbsoluteTime", "[", "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5222466960362115`*^9, 3.5222467018365436`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["model specification", "Section",
 CellChangeTimes->{{3.513531803717296*^9, 3.513531806022168*^9}, {
  3.51767999292592*^9, 3.5176799960260973`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"robotInfo", "=", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{"{", 
     RowBox[{
     "Lc", ",", "Lt", ",", "LT", ",", "rc", ",", "rt", ",", "rT", ",", "mh", 
      ",", "mt", ",", "mc", ",", "g"}], "}"}], ",", "\[IndentingNewLine]", 
    RowBox[{"{", 
     RowBox[{
     "347.98", ",", "261.112", ",", "0", ",", "282.37", ",", "128.15", ",", 
      "9.97", ",", 
      StyleBox["804.83",
       FontWeight->"Plain"], ",", "606.15", ",", "213.79", ",", "9810"}], 
     "}"}]}], "}"}]}]], "Input",
 CellChangeTimes->{{3.5212045686835737`*^9, 3.521204606993765*^9}, {
   3.521204700067089*^9, 3.521204780986717*^9}, {3.5212145569110236`*^9, 
   3.5212145580280876`*^9}, {3.5212214210366507`*^9, 
   3.5212215159440784`*^9}, {3.521221549532*^9, 3.521221581995857*^9}, {
   3.5212225825050826`*^9, 3.5212225872403536`*^9}, {3.5212910875692406`*^9, 
   3.5212910900933847`*^9}, {3.5212911767343407`*^9, 
   3.5212911783804345`*^9}, {3.521291778077735*^9, 3.5212918161939154`*^9}, {
   3.521291985634607*^9, 3.521291991076918*^9}, {3.5212922081883364`*^9, 
   3.5212922159737816`*^9}, {3.5214849738620605`*^9, 3.521484978075301*^9}, {
   3.5214860083742313`*^9, 3.5214860164166913`*^9}, {3.522244545041182*^9, 
   3.522244546126244*^9}, {3.522244593734967*^9, 3.5222446610148153`*^9}, {
   3.522245182408637*^9, 3.5222451980385313`*^9}, {3.5222452480633926`*^9, 
   3.5222452884067*^9}, {3.5222453190144506`*^9, 3.522245320543538*^9}, {
   3.5222453895034823`*^9, 3.522245390398534*^9}, {3.5222454580864053`*^9, 
   3.5222454737583017`*^9}, {3.522508873288*^9, 3.522508878436*^9}, {
   3.52251692506606*^9, 3.5225169259961133`*^9}, {3.5226866514206*^9, 
   3.5226866515376*^9}, {3.5226870047536*^9, 3.5226870474105997`*^9}, {
   3.5226870857056*^9, 3.5226870879856*^9}, {3.5226871451626*^9, 
   3.5226871878106003`*^9}, 3.5230284498409986`*^9, {3.5230306460814857`*^9, 
   3.5230306462784967`*^9}, 3.523375153495903*^9, {3.5239834564467072`*^9, 
   3.5239834661022596`*^9}, {3.523984231265024*^9, 3.523984232481094*^9}, {
   3.5239849634039*^9, 3.52398496409194*^9}, 3.5239872789323406`*^9, {
   3.52398733818773*^9, 3.5239873424349732`*^9}, {3.5239874168672304`*^9, 
   3.5239874206984496`*^9}, {3.5239887549537644`*^9, 3.5239887551847773`*^9}, 
   3.52398922115843*^9, {3.5239892813588734`*^9, 3.523989283325986*^9}, {
   3.5239902778878717`*^9, 3.5239902785899115`*^9}, {3.5239955479933043`*^9, 
   3.523995556641799*^9}, {3.524246824429494*^9, 3.524246849028901*^9}, 
   3.524247077035942*^9, {3.5242480919719934`*^9, 3.524248100636489*^9}, {
   3.524248357066156*^9, 3.524248363905547*^9}, {3.5244002051058645`*^9, 
   3.5244002111532106`*^9}, {3.5244993533445044`*^9, 
   3.5244993688743925`*^9}, {3.524501441984967*^9, 3.5245014440170836`*^9}, {
   3.524502295655794*^9, 3.5245022998480344`*^9}, 3.5245104623769045`*^9, {
   3.5245105245994635`*^9, 3.524510543760559*^9}, {3.5245882608313823`*^9, 
   3.52458826166043*^9}, {3.5245883428200717`*^9, 3.524588343628118*^9}, {
   3.525090259555007*^9, 3.525090265970374*^9}, {3.525461165478421*^9, 
   3.5254611699576774`*^9}, {3.525812138101346*^9, 3.525812158210496*^9}, {
   3.525813345869426*^9, 3.5258133608112807`*^9}, 3.5258134002115345`*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"Lc", "Lt", "LT", "rc", "rt", "rT", "mh", "mt", "mc", "g"},
     {"347.98`", "261.112`", "0", "282.37`", "128.15`", "9.97`", "804.83`", 
      "606.15`", "213.79`", "9810"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5258121627467556`*^9, 3.525812224289275*^9, 3.5258124934506707`*^9, 
   3.5258125832478065`*^9, {3.5258133872237916`*^9, 3.5258134056648464`*^9}, 
   3.5258244670784554`*^9, {3.5271754673244963`*^9, 3.527175504030538*^9}, 
   3.5271784302666597`*^9, 3.527178494657889*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Lc", "/.", "constsubs"}]], "Input",
 CellChangeTimes->{{3.5258285083016005`*^9, 3.525828511605789*^9}}],

Cell[BoxData[
 FormBox[
  FractionBox["17399", "50000"], TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5258285119318075`*^9, {3.5271754674753103`*^9, 3.52717550409967*^9}, 
   3.527178430435211*^9, 3.5271784960307913`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Lt", "/.", "constsubs"}]], "Input",
 CellChangeTimes->{{3.525828513972924*^9, 3.5258285177301397`*^9}}],

Cell[BoxData[
 FormBox[
  FractionBox["32639", "125000"], TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5258285181971664`*^9, {3.527175467505555*^9, 3.527175504147051*^9}, 
   3.527178430482325*^9, 3.527178497457075*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"constsubs", "=", 
  RowBox[{
   RowBox[{"(", 
    RowBox[{
     RowBox[{
      RowBox[{"#1", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}], 
      "\[Rule]", 
      RowBox[{"Rationalize", "[", 
       FractionBox[
        RowBox[{"#1", "\[LeftDoubleBracket]", "2", "\[RightDoubleBracket]"}], 
        "1000"], "]"}]}], "&"}], ")"}], "/@", 
   RowBox[{"Transpose", "[", "robotInfo", "]"}]}]}]], "Input",
 CellChangeTimes->{{3.5197506180672736`*^9, 3.519750623492584*^9}, {
   3.5197506874442415`*^9, 3.519750758152286*^9}, {3.51975089075187*^9, 
   3.519750913356163*^9}, 3.521204776350452*^9, {3.5212215282457824`*^9, 
   3.521221530862932*^9}, {3.522245362849958*^9, 3.522245370044369*^9}, 
   3.5230284621307015`*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"{", 
   RowBox[{
    RowBox[{"Lc", "\[Rule]", 
     FractionBox["17399", "50000"]}], ",", 
    RowBox[{"Lt", "\[Rule]", 
     FractionBox["32639", "125000"]}], ",", 
    RowBox[{"LT", "\[Rule]", "0"}], ",", 
    RowBox[{"rc", "\[Rule]", 
     FractionBox["28237", "100000"]}], ",", 
    RowBox[{"rt", "\[Rule]", 
     FractionBox["2563", "20000"]}], ",", 
    RowBox[{"rT", "\[Rule]", 
     FractionBox["997", "100000"]}], ",", 
    RowBox[{"mh", "\[Rule]", 
     FractionBox["80483", "100000"]}], ",", 
    RowBox[{"mt", "\[Rule]", 
     FractionBox["12123", "20000"]}], ",", 
    RowBox[{"mc", "\[Rule]", 
     FractionBox["21379", "100000"]}], ",", 
    RowBox[{"g", "\[Rule]", 
     FractionBox["981", "100"]}]}], "}"}], TraditionalForm]], "Output",
 CellChangeTimes->{
  3.525812173005342*^9, 3.525812224626295*^9, 3.525812493476672*^9, 
   3.5258125833348117`*^9, {3.525813387282795*^9, 3.5258134057068486`*^9}, 
   3.525824467126458*^9, {3.52717546753502*^9, 3.527175504181251*^9}, 
   3.527178430518162*^9, 3.5271784993581676`*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"ndof", " ", "=", " ", "5"}], ";"}]], "Input",
 CellChangeTimes->{{3.5158694971113553`*^9, 3.51586950751295*^9}, {
  3.515885996605161*^9, 3.5158859966675615`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"mm", "=", 
   RowBox[{
    RowBox[{"{", 
     RowBox[{"mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc"}], "}"}], "/.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"statesubs", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"x", "[", "i", "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["\[Theta]", "i"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"x", "[", 
         RowBox[{"i", "+", "ndof"}], "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], "]"}]}], 
  ";"}]}], "Input",
 CellChangeTimes->{{3.504464931380743*^9, 3.504464938859278*^9}, {
   3.50446501108097*^9, 3.504465011431731*^9}, {3.504465051759923*^9, 
   3.504465055474*^9}, {3.504617746752343*^9, 3.5046177488414*^9}, 
   3.504747296129977*^9, 3.50487586421991*^9, {3.504971551457563*^9, 
   3.50497170345489*^9}, {3.504971751954021*^9, 3.504971771812788*^9}, {
   3.5052271115942125`*^9, 3.5052271200806274`*^9}, {3.505231180364644*^9, 
   3.505231192454666*^9}, {3.50523314771557*^9, 3.5052331520835776`*^9}, {
   3.513011680554058*^9, 3.513011725186837*^9}, 3.51301182562513*^9, {
   3.513012344045216*^9, 3.513012345041097*^9}, {3.5158498204093294`*^9, 
   3.515849822884471*^9}, {3.515850217828061*^9, 3.5158502194581537`*^9}, {
   3.515850546875881*^9, 3.5158505634158273`*^9}, {3.515850617926945*^9, 
   3.515850624852341*^9}, {3.5158506825046387`*^9, 3.515850684811771*^9}, {
   3.5158510414861712`*^9, 3.5158510548999386`*^9}, {3.5158522964848585`*^9, 
   3.5158522987139854`*^9}, {3.5158524456553907`*^9, 
   3.5158524490995874`*^9}, {3.5158693992257566`*^9, 3.515869425043233*^9}, {
   3.515869486273735*^9, 3.515869512859256*^9}, {3.516707936034129*^9, 
   3.516707940618391*^9}, 3.516708850637441*^9}],

Cell["define inertia matrices", "Text",
 CellChangeTimes->{{3.516708878711047*^9, 3.516708881662216*^9}}],

Cell["define inertia matrices", "Text",
 CellChangeTimes->{{3.516708878711047*^9, 3.516708881662216*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"For", "[", 
   RowBox[{
    RowBox[{"i", "=", "1"}], ",", 
    RowBox[{"i", "\[LessEqual]", 
     RowBox[{"Length", "[", "mm", "]"}]}], ",", 
    RowBox[{"i", "++"}], ",", 
    RowBox[{
     SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
     RowBox[{
      RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
      " ", 
      RowBox[{"DiagonalMatrix", "[", 
       RowBox[{"{", 
        RowBox[{"1", ",", "1", ",", "1"}], "}"}], "]"}]}]}]}], "]"}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.516708870317567*^9, 3.5167089298489723`*^9}, {
  3.5167089599876957`*^9, 3.516708960498725*^9}, {3.517697852846449*^9, 
  3.5176978537425003`*^9}}],

Cell["\<\
add inertia for torso (2.5 cm radius), the x,z terms will not show up in the \
EOM for this 2d model\
\>", "Text",
 CellChangeTimes->{{3.5167089723744044`*^9, 3.5167089780147266`*^9}, {
  3.516709130118427*^9, 3.5167091434381886`*^9}, {3.5167094517588234`*^9, 
  3.516709461902404*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "1"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1967374.33", "0", "12.37"},
         {"0", "1946798.09", 
          RowBox[{"-", "19.96"}]},
         {"12.37", 
          RowBox[{"-", "19.96"}], "119696.29"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "2"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"6494948.89", "58.80", "2024.23"},
         {"58.80", "6396011.40", "146697.91"},
         {"2024.23", "146697.91", "418371.07"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "3"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"3730232.81", 
          RowBox[{"-", "7.44"}], "1948.96"},
         {
          RowBox[{"-", "7.44"}], "518271.83", 
          RowBox[{"-", "1348.28"}]},
         {"1948.96", 
          RowBox[{"-", "1348.28"}], "3577190.08"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "4"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"6494948.89", 
          RowBox[{"-", "60.11"}], "1985.63"},
         {
          RowBox[{"-", "60.11"}], "6396011.38", 
          RowBox[{"-", "146697.91"}]},
         {"1985.63", 
          RowBox[{"-", "146697.91"}], "418371.06"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "5"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1967374.33", "0", 
          RowBox[{"-", "12.37"}]},
         {"0", "1946798.091", "19.96"},
         {
          RowBox[{"-", "12.37"}], "19.96", "119696.29"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.517697279088632*^9, 3.5176972829248514`*^9}, {
   3.5176973500646915`*^9, 3.5176974887036214`*^9}, {3.5176976110556197`*^9, 
   3.517697611375638*^9}, {3.520454104179551*^9, 3.5204542318028507`*^9}, {
   3.520454298738679*^9, 3.520454298807683*^9}, {3.520787636320943*^9, 
   3.520787636928978*^9}, {3.5207888553356667`*^9, 3.520788855551679*^9}, {
   3.5212216150807495`*^9, 3.5212218001203327`*^9}, {3.52122184116868*^9, 
   3.5212218551204786`*^9}, {3.5212923587829494`*^9, 
   3.5212923937079477`*^9}, {3.5214873144856634`*^9, 3.521487314869685*^9}, {
   3.5222446851841974`*^9, 3.5222446853692083`*^9}, {3.5222447190471344`*^9, 
   3.522244739155285*^9}, {3.522244896148264*^9, 3.5222449131612372`*^9}, {
   3.5225090343112*^9, 3.5225091254776*^9}, {3.5226873222595997`*^9, 
   3.5226873950976*^9}, 3.523027420015806*^9, {3.5233760202527485`*^9, 
   3.523376091120827*^9}, {3.5233762610257463`*^9, 3.523376327448327*^9}, {
   3.523376418297227*^9, 3.523376428528584*^9}, {3.5239880730617623`*^9, 
   3.5239880821322813`*^9}, {3.523988123796664*^9, 3.523988137267435*^9}, {
   3.523988338051919*^9, 3.5239883852996216`*^9}, {3.523988618537962*^9, 
   3.5239886371700277`*^9}, {3.5239889249444876`*^9, 
   3.5239889927513657`*^9}, {3.5242480213809557`*^9, 
   3.5242480706757755`*^9}, {3.5244990481420474`*^9, 3.524499082506013*^9}, {
   3.525812253419942*^9, 3.5258122823085938`*^9}, {3.525812449570161*^9, 
   3.525812454153423*^9}, 3.5258125549041853`*^9, {3.5258133755181217`*^9, 
   3.5258133759191446`*^9}}],

Cell[BoxData[
 RowBox[{"For", "[", 
  RowBox[{
   RowBox[{"i", "=", "1"}], ",", 
   RowBox[{"i", "\[LessEqual]", "5"}], ",", 
   RowBox[{"i", "++"}], ",", 
   RowBox[{
    SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Join", "[", 
       RowBox[{
        SubscriptBox["\[ScriptCapitalM]", "i"], ",", 
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", "2"}], "]"}], ",", 
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", 
        SubscriptBox["\[ScriptCapitalI]", "i"], ",", "2"}], "]"}], ",", "1"}],
      "]"}]}]}], "]"}]], "Input"],

Cell[BoxData[
 RowBox[{
  RowBox[{"p0", "=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["p", "x"], "'"}], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["p", "z"], "'"}], "[", "t", "]"}], "\[Rule]", "0"}]}], 
    "}"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.513532355258184*^9, 3.513532394514388*^9}, {
  3.5158587679930077`*^9, 3.515858770152131*^9}, {3.5167081862334394`*^9, 
  3.516708188824588*^9}}],

Cell[CellGroupData[{

Cell["generalized coordinates", "Subsection",
 CellChangeTimes->{
  3.504391119483818*^9, {3.513012126441401*^9, 3.51301212962473*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"q", "=", 
   RowBox[{"Table", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"dq", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "q"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"ddq", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "dq"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.504391169851249*^9, 3.504391191970865*^9}, {
   3.5129535326917257`*^9, 3.5129535355376797`*^9}, 3.513011937181409*^9, {
   3.513012138013254*^9, 3.513012139016991*^9}, 3.515858456069166*^9, {
   3.5158695182095623`*^9, 3.515869519316625*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"qe", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}], "}"}]}], "}"}], ",", "q"}], 
    "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"dqe", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "qe"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.513012131467762*^9, 3.513012222132253*^9}, 
   3.51353184821528*^9, 3.5158584591123405`*^9, {3.515859715694213*^9, 
   3.5158597176283236`*^9}}],

Cell[CellGroupData[{

Cell["location and direction of twists", "Subsubsection",
 CellChangeTimes->{
  3.504391222051317*^9, {3.513012004436311*^9, 3.513012017657574*^9}}],

Cell["positive z direction, ", "Text",
 CellChangeTimes->{{3.504895806007092*^9, 3.504895813878101*^9}, {
  3.504899673358274*^9, 3.504899680392001*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "0"], "=", 
   RowBox[{"{", 
    RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "px"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "pz"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q1"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q2"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q3"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q4"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q5"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.504391239055382*^9, 3.504391280546184*^9}, {
   3.504393690323417*^9, 3.504393694642943*^9}, {3.504394354402188*^9, 
   3.504394382731842*^9}, 3.504394560552597*^9, {3.504733962343795*^9, 
   3.50473399215855*^9}, {3.504734047207357*^9, 3.504734112723113*^9}, {
   3.504895790317638*^9, 3.504895793671604*^9}, {3.504895835178937*^9, 
   3.504895837468827*^9}, {3.50489969948502*^9, 3.504899711184298*^9}, {
   3.504901355868927*^9, 3.504901367126576*^9}, {3.504901644110264*^9, 
   3.504901651461383*^9}, {3.5125072468769054`*^9, 3.5125072492793093`*^9}, {
   3.512510523268261*^9, 3.512510626680843*^9}, {3.512510703620178*^9, 
   3.5125107228706117`*^9}, {3.5125107694990935`*^9, 
   3.5125107706690955`*^9}, {3.512510874416078*^9, 3.5125108760228806`*^9}, {
   3.5125109282169724`*^9, 3.5125109524594145`*^9}, 3.512511205857059*^9, {
   3.5125116217689886`*^9, 3.5125116245925937`*^9}, {3.512512838505127*^9, 
   3.5125128440899363`*^9}, 3.5125154382821627`*^9, 3.512515631401702*^9, {
   3.512515675315779*^9, 3.512515677234583*^9}, {3.512515713489046*^9, 
   3.51251571545465*^9}, {3.5125157856379766`*^9, 3.5125157895691843`*^9}, {
   3.5125160948139267`*^9, 3.512516101631139*^9}, {3.5125163059110966`*^9, 
   3.512516337906753*^9}, 3.5125169799917154`*^9, 3.512517350494175*^9, {
   3.5125175771075783`*^9, 3.512517615811246*^9}, {3.5125182182931213`*^9, 
   3.5125182275127373`*^9}, {3.513011948790205*^9, 3.513011986836761*^9}, {
   3.513012028383106*^9, 3.513012030110642*^9}, {3.513531858718806*^9, 
   3.513531893067239*^9}, {3.513869388669607*^9, 3.513869391129299*^9}, {
   3.513956274948754*^9, 3.513956281701611*^9}, {3.5155018106921597`*^9, 
   3.515501813883342*^9}, {3.515501850954462*^9, 3.5155018542846527`*^9}, {
   3.5158584657537203`*^9, 3.515858513238436*^9}, {3.515869263882015*^9, 
   3.5158692653601*^9}, {3.5158703231956043`*^9, 3.515870326599799*^9}, {
   3.515886006277178*^9, 3.5158860120335884`*^9}, {3.5167081937628703`*^9, 
   3.5167082227325273`*^9}, 3.51670884415207*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["base configuration", "Subsubsection",
 CellChangeTimes->{
  3.504391314503995*^9, {3.513011998415469*^9, 3.513011998449444*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["g", 
    SubscriptBox["sl", "1"]], "[", "0", "]"}], "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", 
        FractionBox[
         RowBox[{"-", "33"}], "100000"], ",", "rc"}], "}"}]}], "]"}], "/.", 
    "\[InvisibleSpace]", "constsubs"}], "]"}]}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["g", 
    SubscriptBox["sl", "2"]], "[", "0", "]"}], "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{
        FractionBox["2", "100000"], ",", 
        FractionBox[
         RowBox[{"-", "381"}], "100000"], ",", 
        RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
    "\[InvisibleSpace]", "constsubs"}], "]"}]}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "3"]], "[", "0", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{
         FractionBox[
          RowBox[{"-", "24"}], "100000"], ",", 
         FractionBox[
          RowBox[{"-", "4"}], "100000"], ",", 
         RowBox[{"Lc", "+", "Lt", "+", "rT"}]}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     SubscriptBox["g", 
      SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{
          FractionBox[
           RowBox[{"-", "24"}], "100000"], ",", 
          FractionBox[
           RowBox[{"-", "4"}], "100000"], ",", 
          RowBox[{"Lc", "+", "Lt", "+", "Lb"}]}], "}"}]}], "]"}], "/.", 
      "\[InvisibleSpace]", "constsubs"}], "]"}]}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["g", 
    SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{
        FractionBox["2", "100000"], ",", 
        FractionBox["381", "100000"], ",", 
        RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
    "\[InvisibleSpace]", "constsubs"}], "]"}]}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["g", 
    SubscriptBox["sl", "5"]], "[", "0", "]"}], "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", 
        FractionBox["33", "100000"], ",", "rc"}], "}"}]}], "]"}], "/.", 
    "\[InvisibleSpace]", "constsubs"}], "]"}]}]}], "Input",
 CellChangeTimes->{
  3.504391344841228*^9, {3.504393660794399*^9, 3.504393663580984*^9}, {
   3.504393700919354*^9, 3.504393704560237*^9}, 3.504477320678662*^9, 
   3.504477372811738*^9, {3.5125106491760826`*^9, 3.5125106821701403`*^9}, {
   3.5125108958281155`*^9, 3.512510897871719*^9}, {3.513012037683773*^9, 
   3.513012046816517*^9}, {3.51353190099301*^9, 3.513531902632749*^9}, {
   3.5158497160803623`*^9, 3.5158497388296633`*^9}, {3.5158505076696386`*^9, 
   3.5158505174832*^9}, {3.51585069440932*^9, 3.5158507070580435`*^9}, {
   3.5158510492876177`*^9, 3.5158510493766227`*^9}, {3.5158515449339666`*^9, 
   3.515851552726413*^9}, {3.5158516115196805`*^9, 3.5158516208742156`*^9}, {
   3.515852304074292*^9, 3.515852316172984*^9}, {3.5158524561829925`*^9, 
   3.515852474285028*^9}, {3.515859753607381*^9, 3.5158597547104445`*^9}, {
   3.515869286854329*^9, 3.5158692891464605`*^9}, {3.5158703778677316`*^9, 
   3.515870383570058*^9}, {3.5158860185856*^9, 3.5158860205200033`*^9}, {
   3.516707969937068*^9, 3.5167079805786767`*^9}, {3.5167082033634195`*^9, 
   3.516708212488941*^9}, 3.516708841559922*^9, {3.5167096385125055`*^9, 
   3.5167096393665543`*^9}, {3.517679210208151*^9, 3.5176792179195924`*^9}, {
   3.5176856517675877`*^9, 3.5176856652333574`*^9}, {3.5176858174780655`*^9, 
   3.5176858538481455`*^9}, {3.5212226605105443`*^9, 
   3.5212226803056765`*^9}, {3.5212227191418977`*^9, 3.52122290328043*^9}, {
   3.521292043444914*^9, 3.5212920980150347`*^9}, {3.521292133845084*^9, 
   3.5212921713812313`*^9}, {3.5212922405391865`*^9, 3.521292296637395*^9}, {
   3.5214850234198947`*^9, 3.521485050627451*^9}, {3.5222465278455915`*^9, 
   3.5222465426854405`*^9}, {3.5245004568416204`*^9, 
   3.5245004677162423`*^9}, {3.5245014065889425`*^9, 
   3.5245014068999605`*^9}, {3.524501486108491*^9, 3.5245014880206003`*^9}, {
   3.5245022041465607`*^9, 3.524502211992009*^9}, {3.524510625029207*^9, 
   3.5245106719988937`*^9}, 3.52509193018268*^9, {3.527178668195559*^9, 
   3.5271786786368523`*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"1", "0", "0", "0"},
     {"0", "1", "0", 
      RowBox[{"-", 
       FractionBox["33", "100000"]}]},
     {"0", "0", "1", 
      FractionBox["28237", "100000"]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5271754676564417`*^9, 3.527175504273881*^9, 3.527178430674592*^9, {
   3.527178679326642*^9, 3.527178693978397*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"1", "0", "0", 
      FractionBox["1", "50000"]},
     {"0", "1", "0", 
      RowBox[{"-", 
       FractionBox["381", "100000"]}]},
     {"0", "0", "1", 
      FractionBox["47613", "100000"]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5271754676564417`*^9, 3.527175504273881*^9, 3.527178430674592*^9, {
   3.527178679326642*^9, 3.527178693980323*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"1", "0", "0", 
      RowBox[{"-", 
       FractionBox["3", "12500"]}]},
     {"0", "1", "0", 
      RowBox[{"-", 
       FractionBox["1", "25000"]}]},
     {"0", "0", "1", 
      FractionBox["309531", "500000"]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5271754676564417`*^9, 3.527175504273881*^9, 3.527178430674592*^9, {
   3.527178679326642*^9, 3.527178693982053*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"1", "0", "0", 
      FractionBox["1", "50000"]},
     {"0", "1", "0", 
      FractionBox["381", "100000"]},
     {"0", "0", "1", 
      FractionBox["47613", "100000"]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5271754676564417`*^9, 3.527175504273881*^9, 3.527178430674592*^9, {
   3.527178679326642*^9, 3.527178693983862*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"1", "0", "0", "0"},
     {"0", "1", "0", 
      FractionBox["33", "100000"]},
     {"0", "0", "1", 
      FractionBox["28237", "100000"]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5271754676564417`*^9, 3.527175504273881*^9, 3.527178430674592*^9, {
   3.527178679326642*^9, 3.527178693985833*^9}}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "1"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.516708682282812*^9, 3.5167087197409544`*^9}, 
   3.5167087910620337`*^9, 3.516708838777763*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "2"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.5167087252072673`*^9, 3.5167087354608536`*^9}, 
   3.516708790053976*^9, 3.5167088387897635`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "3"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.5167087432522993`*^9, 3.5167087881018643`*^9}, 
   3.5167088388017645`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "4"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.5167087493406477`*^9, 3.5167087863827662`*^9}, 
   3.5167088388117647`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "5"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q5"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.5167087714209104`*^9, 3.516708784944684*^9}, 
   3.5167088388187656`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate the forward kinematics maps", "Subsubsection",
 CellChangeTimes->{
  3.504391365377686*^9, {3.513012058700656*^9, 3.513012059089004*^9}, {
   3.513013029501931*^9, 3.513013029568491*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "1"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "2"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "3"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "4"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "5"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q5"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.504391384027502*^9, 3.504391388680142*^9}, 
   3.504392263056063*^9, {3.504395252151757*^9, 3.504395255570185*^9}, {
   3.504733906298723*^9, 3.504733948089162*^9}, {3.504734137831782*^9, 
   3.504734159403184*^9}, {3.504734203476301*^9, 3.504734287191608*^9}, {
   3.504734420873801*^9, 3.504734554212083*^9}, {3.50473502117398*^9, 
   3.504735032877724*^9}, 3.504817876603461*^9, {3.5125107939599366`*^9, 
   3.5125108014011497`*^9}, 3.5125111628601837`*^9, {3.5125113835345707`*^9, 
   3.512511397605796*^9}, {3.513012063161425*^9, 3.513012087266791*^9}, {
   3.513012234208855*^9, 3.513012272695982*^9}, {3.513531912661093*^9, 
   3.513531929595589*^9}, {3.513532847587819*^9, 3.513532852574812*^9}, {
   3.515849743378924*^9, 3.515849789318551*^9}, {3.515850519247301*^9, 
   3.515850536609294*^9}, {3.5158507106542487`*^9, 3.5158507498544908`*^9}, {
   3.5158523190301476`*^9, 3.515852341076409*^9}, {3.5158524799643526`*^9, 
   3.515852528781145*^9}, {3.515858539342929*^9, 3.51585859216195*^9}, {
   3.515869305864416*^9, 3.515869315460965*^9}, {3.5158695563657446`*^9, 
   3.515869562919119*^9}, {3.5158703685702*^9, 3.5158703707433243`*^9}, 
   3.5158861184257755`*^9, {3.516707985323948*^9, 3.5167080036019936`*^9}, {
   3.516708230348963*^9, 3.516708236126293*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stf"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stk"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "hip"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"Lc", "+", "Lt"}]}], "}"}]}], "]"}], "/.", "constsubs"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "torso"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"Lc", "+", "Lt", "+", "LT"}]}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsk"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";", 
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504391344841228*^9, {3.504393660794399*^9, 3.504393663580984*^9}, {
   3.504393700919354*^9, 3.504393704560237*^9}, 3.504477320678662*^9, 
   3.504477372811738*^9, {3.5125106491760826`*^9, 3.5125106821701403`*^9}, {
   3.5125108958281155`*^9, 3.512510897871719*^9}, {3.513012037683773*^9, 
   3.513012046816517*^9}, {3.51353190099301*^9, 3.513531902632749*^9}, {
   3.513875671984485*^9, 3.513875737139051*^9}, {3.515858621125607*^9, 
   3.51585862956809*^9}, {3.5158693201992364`*^9, 3.515869321718323*^9}, {
   3.515886061969276*^9, 3.5158860734820957`*^9}, {3.516708239115464*^9, 
   3.5167082431056924`*^9}, {3.517679572905896*^9, 3.5176795767731175`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "stf"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "stk"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "hip"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "torso"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "nsk"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";", 
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q5"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504391344841228*^9, {3.504393660794399*^9, 3.504393663580984*^9}, {
   3.504393700919354*^9, 3.504393704560237*^9}, 3.504477320678662*^9, 
   3.504477372811738*^9, {3.5125106491760826`*^9, 3.5125106821701403`*^9}, {
   3.5125108958281155`*^9, 3.512510897871719*^9}, {3.513012037683773*^9, 
   3.513012046816517*^9}, {3.51353190099301*^9, 3.513531902632749*^9}, {
   3.513875795389027*^9, 3.513875935660706*^9}, 3.513876139346497*^9, {
   3.5158497919627028`*^9, 3.515849794085824*^9}, {3.515858606539773*^9, 
   3.5158586531084366`*^9}, {3.515869325403534*^9, 3.515869328852731*^9}, {
   3.5158695423689437`*^9, 3.515869545677133*^9}, {3.5158697121476545`*^9, 
   3.5158697210631647`*^9}, {3.5158702933078947`*^9, 
   3.5158703090517955`*^9}, {3.5158860863521185`*^9, 3.515886097818139*^9}, {
   3.515886139454612*^9, 3.5158861478630266`*^9}, {3.516708246161867*^9, 
   3.5167082520592046`*^9}, 3.5194838091162057`*^9, 3.525812570997106*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["aniplot", "Subsubsection",
 CellChangeTimes->{{3.51387566509846*^9, 3.513875667385709*^9}, {
  3.513876273981033*^9, 3.51387627490601*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"pos", "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "3"}], "}"}], ",", 
        RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "3"}], "}"}], ",", 
        RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "3"}], "}"}], ",", 
        RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "3"}], "}"}], ",", 
        RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "3"}], "}"}], ",", 
        RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "3"}], "}"}], ",", 
        RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "3"}], "}"}], ",", 
        RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", "2"}], 
     "]"}], "]"}]}], "\[IndentingNewLine]"}]], "Input",
 CellChangeTimes->{{3.513875953621905*^9, 3.513876010689337*^9}, {
  3.513876043248599*^9, 3.51387610532577*^9}, {3.513876169675573*^9, 
  3.513876183706996*^9}, {3.513887655923167*^9, 3.513887658548081*^9}, {
  3.513888227999695*^9, 3.513888239050703*^9}, {3.5158587000371203`*^9, 
  3.51585871925922*^9}, {3.5158693371802073`*^9, 3.5158693660608597`*^9}, {
  3.5158696092147675`*^9, 3.5158696155961323`*^9}, {3.515886157784644*^9, 
  3.515886172869871*^9}, {3.516708254867365*^9, 3.5167082553383923`*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0", 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"17399", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], 
        "50000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
          ")"}]}], "250000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
          ")"}]}], "250000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
          ")"}]}], "250000"]}], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", "685"}], " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
          RowBox[{"514", " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"sin", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}], "-", 
             
             RowBox[{"sin", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
            ")"}]}]}], ")"}]}], "250000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", "685"}], " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
          RowBox[{"514", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
          
          RowBox[{"514", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"]},
     {"0", 
      FractionBox[
       RowBox[{"17399", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "50000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"cos", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}], "-", 
             
             RowBox[{"cos", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
            ")"}]}]}], ")"}]}], "250000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
          
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
          
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"]}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.513876117752395*^9, 3.513876148979501*^9, {3.513876184741597*^9, 
   3.513876194630618*^9}, 3.513876324902807*^9, 3.51388708682703*^9, 
   3.513887485398372*^9, 3.513887667491836*^9, 3.513888249789927*^9, 
   3.513946474966597*^9, 3.513946529009685*^9, 3.513947877926773*^9, 
   3.513948014898516*^9, 3.513948071368912*^9, 3.513948136669405*^9, 
   3.513950337007029*^9, 3.513956183716542*^9, 3.513956289564141*^9, 
   3.5139840610782843`*^9, 3.514047896156836*^9, 3.514054924782146*^9, 
   3.514054957962686*^9, 3.514055242514401*^9, 3.514056538375741*^9, 
   3.514058950609707*^9, {3.514213188392887*^9, 3.5142132086290445`*^9}, 
   3.515336409379179*^9, 3.515344266283826*^9, 3.515344306978285*^9, 
   3.515345655715322*^9, 3.515435489566894*^9, 3.5154355925888*^9, 
   3.5154971631033325`*^9, 3.5154972084979286`*^9, 3.51550181630048*^9, 
   3.5155018573208265`*^9, 3.5155188182249346`*^9, 3.5155213098694487`*^9, {
   3.515540867167141*^9, 3.515540892482759*^9}, 3.515840725617695*^9, 
   3.515841619495264*^9, 3.515849855176318*^9, 3.5158501223145976`*^9, {
   3.515850555586379*^9, 3.5158505674750595`*^9}, 3.515850793609994*^9, 
   3.5158510707118425`*^9, 3.515851562106949*^9, 3.51585162532547*^9, {
   3.5158523515610085`*^9, 3.515852375834397*^9}, {3.5158525431109643`*^9, 
   3.5158525602519445`*^9}, {3.515852718972023*^9, 3.5158527342428966`*^9}, 
   3.5158527764453106`*^9, 3.515854903222955*^9, 3.515854975879111*^9, 
   3.515855203251116*^9, 3.515855237400069*^9, 3.515859444925726*^9, 
   3.5158595628024683`*^9, {3.5158595937562385`*^9, 3.515859608460079*^9}, 
   3.5158601640478573`*^9, 3.515860210029487*^9, 3.515860490811547*^9, 
   3.515860725004942*^9, 3.515861304004059*^9, 3.515861502963439*^9, 
   3.5158621626381702`*^9, 3.5158641685735803`*^9, 3.5158646292879314`*^9, 
   3.5158657398934546`*^9, 3.515865802016008*^9, 3.5158700556943045`*^9, 
   3.515870286489505*^9, 3.5158703441838045`*^9, 3.515870389161378*^9, 
   3.5158729265245066`*^9, 3.515886470702795*^9, 3.5158865828981924`*^9, 
   3.5158872848538523`*^9, 3.515887375614812*^9, 3.515888954866113*^9, 
   3.515889939652219*^9, 3.515890130251956*^9, 3.515948182034857*^9, 
   3.5159487307398214`*^9, 3.51594882123558*^9, {3.5159532382800736`*^9, 
   3.5159532636085224`*^9}, 3.515953586621998*^9, 3.515972556644344*^9, 
   3.515985676629764*^9, 3.5167069702268877`*^9, 3.5167086567063494`*^9, 
   3.516709645884927*^9, 3.5167097164419622`*^9, 3.516711205420127*^9, 
   3.5167125419505725`*^9, 3.5167318295131783`*^9, 3.5167514874042244`*^9, 
   3.517679147930589*^9, 3.5176795810543623`*^9, 3.517679670054453*^9, 
   3.517680000244339*^9, 3.517685913913581*^9, 3.517697962236706*^9, 
   3.517698399869737*^9, 3.517698513959263*^9, 3.519062141330331*^9, 
   3.519482383130644*^9, {3.5194856721237636`*^9, 3.5194856864215813`*^9}, 
   3.519487959863615*^9, 3.5194879981608057`*^9, 3.51948811677659*^9, 
   3.519488417368783*^9, 3.5194906624368863`*^9, 3.5194909731076555`*^9, 
   3.519565562573549*^9, 3.5196479562977695`*^9, 3.5196482476144323`*^9, 
   3.5258124958258066`*^9, 3.5258125856929464`*^9, {3.5258133887898808`*^9, 
   3.525813408146988*^9}, 3.5258244699826217`*^9, 3.527175470283524*^9, 
   3.5271755068253927`*^9, 3.527178433251246*^9, 3.527178696846044*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate center of mass", "Subsubsection",
 CellChangeTimes->{{3.515859381163079*^9, 3.5158593881694794`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["p", "COM"], "=", 
  RowBox[{
   RowBox[{
    RowBox[{
     RowBox[{"(", 
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], 
        RowBox[{"Length", "[", "mm", "]"}]], 
       RowBox[{
        RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
        " ", 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
         RowBox[{"(", 
          RowBox[{"Total", "[", "mm", "]"}], ")"}]}]}]}], ")"}], "/.", 
     "constsubs"}], "//.", "p0"}], "//", "Simplify"}]}]], "Input",
 CellChangeTimes->{{3.515859389894578*^9, 3.515859428264773*^9}, {
   3.515859465154883*^9, 3.5158594906983433`*^9}, {3.515859551458819*^9, 
   3.515859552570883*^9}, {3.515859601883703*^9, 3.5158596025067387`*^9}, {
   3.515860689352903*^9, 3.5158607032496977`*^9}, 3.5158888908592005`*^9, {
   3.5158889225272565`*^9, 3.515888940763688*^9}, {3.515889859873678*^9, 
   3.5158898662696896`*^9}, {3.5159487112085867`*^9, 3.515948711738988*^9}, {
   3.5159488047463512`*^9, 3.5159488071019554`*^9}, {3.5159532190899763`*^9, 
   3.5159532199880276`*^9}, {3.515953578551536*^9, 3.51595358036364*^9}, {
   3.5159537466121483`*^9, 3.515953749921338*^9}, {3.5159854643716235`*^9, 
   3.515985465601694*^9}, {3.5167082669630575`*^9, 3.516708282715958*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   FractionBox["1", "122235500000"], 
   RowBox[{"(", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"41834171195", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "-", 
     RowBox[{"25096253337", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
     RowBox[{"401207755", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
     RowBox[{"6820902539", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
     RowBox[{"701338095", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
     RowBox[{"606150", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
     RowBox[{"9657960", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
     RowBox[{"606150", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], ")"}]}],
   TraditionalForm]], "Output",
 CellChangeTimes->{3.525813408420004*^9, 3.5258244701596317`*^9, 
  3.527175470665468*^9, 3.5271755071704407`*^9, 3.5271784338009872`*^9, 
  3.52717869705126*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate the manipulator inertia matrix", "Subsubsection",
 CellChangeTimes->{
  3.504392305735899*^9, 3.513012096077317*^9, {3.516708802974715*^9, 
   3.5167088033737383`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[ScriptCapitalD]e", "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     UnderoverscriptBox["\[Sum]", 
      RowBox[{"i", "=", "1"}], 
      RowBox[{"Length", "[", "mm", "]"}]], 
     RowBox[{
      RowBox[{
       SubscriptBox["\[ScriptCapitalJ]", "i"], "\[Transpose]"}], ".", 
      SubscriptBox["\[ScriptCapitalM]", "i"], ".", 
      SubscriptBox["\[ScriptCapitalJ]", "i"]}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5167096143551235`*^9, 3.516709632103139*^9}}],

Cell["\<\
project out the generalized coordinates defining the position of the stance \
foot to obtain the reduced \[ScriptCapitalD] matrix\
\>", "Text",
 CellChangeTimes->{{3.513012426370111*^9, 3.513012452229178*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"\[ScriptCapitalD]", "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]e", "\[LeftDoubleBracket]", 
     RowBox[{
      RowBox[{"3", ";;", "All"}], ",", 
      RowBox[{"3", ";;", "All"}]}], "\[RightDoubleBracket]"}], "/.", 
    "\[InvisibleSpace]", "p0"}], "]"}]}]], "Input",
 CellChangeTimes->{{3.513012422937284*^9, 3.513012423523879*^9}, {
   3.513012453927046*^9, 3.513012465021281*^9}, {3.513012554051636*^9, 
   3.513012587164631*^9}, {3.51353194017507*^9, 3.513531940256729*^9}, 
   3.515858726761649*^9, {3.515859543127342*^9, 3.5158595437243767`*^9}, 
   3.5158680079901824`*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      FractionBox[
       RowBox[{
        RowBox[{"6849201003061543875", " ", 
         SuperscriptBox[
          RowBox[{"(", 
           RowBox[{
            RowBox[{"34798", " ", 
             RowBox[{"cos", "(", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ")"}]}], "+", 
            "12815"}], ")"}], "2"]}], "+", 
        RowBox[{"363767545544437671", " ", 
         SuperscriptBox[
          RowBox[{"(", 
           RowBox[{
            RowBox[{"130556", " ", 
             RowBox[{"cos", "(", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
            RowBox[{"173990", " ", 
             RowBox[{"cos", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
            "+", "4985"}], ")"}], "2"]}], "+", 
        RowBox[{"273968040122461755", " ", 
         SuperscriptBox[
          RowBox[{"(", 
           RowBox[{
            RowBox[{"130556", " ", 
             RowBox[{"cos", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
            "+", 
            RowBox[{"173990", " ", 
             RowBox[{"cos", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
            "-", "66481"}], ")"}], "2"]}], "+", 
        RowBox[{"96628932273828423", " ", 
         SuperscriptBox[
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"-", "130556"}], " ", 
             RowBox[{"cos", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
            "-", 
            RowBox[{"173990", " ", 
             RowBox[{"cos", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
            "+", 
            RowBox[{"130556", " ", 
             RowBox[{"cos", "(", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
            "32805"}], ")"}], "2"]}], "+", 
        RowBox[{"27396804012246175500", " ", 
         SuperscriptBox[
          RowBox[{"(", 
           RowBox[{
            RowBox[{"17399", " ", 
             RowBox[{"sin", "(", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ")"}]}], "+", 
            "1"}], ")"}], "2"]}], "+", 
        RowBox[{"1455070182177750684", " ", 
         SuperscriptBox[
          RowBox[{"(", 
           RowBox[{
            RowBox[{"65278", " ", 
             RowBox[{"sin", "(", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
            RowBox[{"86995", " ", 
             RowBox[{"sin", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
            "-", "60"}], ")"}], "2"]}], "+", 
        RowBox[{"1095872160489847020", " ", 
         SuperscriptBox[
          RowBox[{"(", 
           RowBox[{
            RowBox[{"65278", " ", 
             RowBox[{"sin", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
            "+", 
            RowBox[{"86995", " ", 
             RowBox[{"sin", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
            "+", "5"}], ")"}], "2"]}], "+", 
        RowBox[{"6234112194578314538268", " ", 
         SuperscriptBox[
          RowBox[{"(", 
           RowBox[{
            RowBox[{"514", " ", 
             RowBox[{"sin", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
            "+", 
            RowBox[{"685", " ", 
             RowBox[{"sin", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
            "+", 
            RowBox[{"514", " ", 
             RowBox[{"sin", "(", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}]}], 
           ")"}], "2"]}], "+", "3870080597176665578268687175"}], 
       "112995149765925000000000000000"], 
      FractionBox[
       RowBox[{
        RowBox[{"9867859916254251392483174655", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"236747797845356528806753860", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"157755098758822200615395325", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"4024931311275503826435792708", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"2681982439906342530261204285", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"413851047162105459574532340", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"275766505161519688529722425", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"413851047162105459574532340", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"238338496504535603762250", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"5699044281131952549009120", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"3797514914956602622637400", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"357681714462281168857800", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"238338496504535603762250", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        "9168426882438801274656182686"}], "56497574882962500000000000000"], 
      RowBox[{
       FractionBox[
        RowBox[{"80241551", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"173990", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+",
            "4985"}], ")"}]}], "5000000000000000"], "-", 
       FractionBox[
        RowBox[{"805949163", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"173990", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-",
            "66481"}], ")"}]}], "5000000000000000"], "+", 
       FractionBox[
        RowBox[{"21379", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
           "32805"}], ")"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"-", "130556"}], " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "-", 
           RowBox[{"173990", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
           "32805"}], ")"}]}], "25000000000000000"], "-", 
       FractionBox[
        RowBox[{"241449", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"65278", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"86995", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-",
            "60"}], ")"}]}], "312500000000000"], "+", 
       FractionBox[
        RowBox[{"12123", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"65278", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"86995", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+",
            "5"}], ")"}]}], "250000000000000"], "+", 
       FractionBox[
        RowBox[{"88619225987", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"685", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}]}], 
          ")"}]}], "3125000000000000"], "+", 
       FractionBox["40050373418", "4519805990637"]}], 
      FractionBox[
       RowBox[{
        RowBox[{"119200714069641172375638", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"158856982758179383418895", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"12256442787481651944990", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"16333975310165236541475", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"24512885574963303889980", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"10592954879532108300", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"14117070218831700750", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        "115636334411063810121713"}], "3346417987500000000000000"], 
      RowBox[{
       FractionBox[
        RowBox[{"140267619", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"173990", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "-", 
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "-", 
           "32805"}], ")"}]}], "5000000000000000"], "-", 
       FractionBox["113", "58044"]}]},
     {
      FractionBox[
       RowBox[{
        RowBox[{"9867859916254251392483174655", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"236747797845356528806753860", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"157755098758822200615395325", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"4024931311275503826435792708", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"2681982439906342530261204285", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"413851047162105459574532340", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"275766505161519688529722425", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"413851047162105459574532340", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"238338496504535603762250", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"5699044281131952549009120", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"3797514914956602622637400", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"357681714462281168857800", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"238338496504535603762250", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        "9168426882438801274656182686"}], "56497574882962500000000000000"], 
      FractionBox[
       RowBox[{
        RowBox[{"118373898922678264403376930", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"2012465655637751913217896354", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"206925523581052729787266170", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"206925523581052729787266170", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"2849522140565976274504560", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"178840857231140584428900", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        "4584213441219400637328091343"}], "28248787441481250000000000000"], 
      FractionBox[
       RowBox[{
        RowBox[{"236747797845356528806753860", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"4024931311275503826435792708", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"413851047162105459574532340", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"827702094324210919149064680", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"5699044281131952549009120", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"357681714462281168857800", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        "3972183726112363476095049533"}], "112995149765925000000000000000"], 
      FractionBox[
       RowBox[{
        RowBox[{"119200714069641172375638", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"12256442787481651944990", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"24512885574963303889980", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"10592954879532108300", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        "115636334411063810121713"}], "3346417987500000000000000"], 
      RowBox[{
       FractionBox[
        RowBox[{"140267619", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "-", 
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "-", 
           "32805"}], ")"}]}], "5000000000000000"], "-", 
       FractionBox["113", "58044"]}]},
     {
      RowBox[{
       FractionBox[
        RowBox[{"80241551", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"173990", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+",
            "4985"}], ")"}]}], "5000000000000000"], "-", 
       FractionBox[
        RowBox[{"805949163", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"173990", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-",
            "66481"}], ")"}]}], "5000000000000000"], "+", 
       FractionBox[
        RowBox[{"21379", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
           "32805"}], ")"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"-", "130556"}], " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "-", 
           RowBox[{"173990", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
           "32805"}], ")"}]}], "25000000000000000"], "-", 
       FractionBox[
        RowBox[{"241449", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"65278", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"86995", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-",
            "60"}], ")"}]}], "312500000000000"], "+", 
       FractionBox[
        RowBox[{"12123", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"65278", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"86995", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+",
            "5"}], ")"}]}], "250000000000000"], "+", 
       FractionBox[
        RowBox[{"88619225987", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"685", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}]}], 
          ")"}]}], "3125000000000000"], "+", 
       FractionBox["40050373418", "4519805990637"]}], 
      FractionBox[
       RowBox[{
        RowBox[{"236747797845356528806753860", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"4024931311275503826435792708", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"413851047162105459574532340", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"827702094324210919149064680", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"5699044281131952549009120", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"357681714462281168857800", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        "3972183726112363476095049533"}], "112995149765925000000000000000"], 
      FractionBox[
       RowBox[{
        RowBox[{"827702094324210919149064680", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
        "3972183726112363476095049533"}], "112995149765925000000000000000"], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "24512885574963303889980"}], " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "-", 
        "115636334411063810121713"}], "3346417987500000000000000"], 
      RowBox[{
       RowBox[{"-", 
        FractionBox[
         RowBox[{"140267619", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"130556", " ", 
             RowBox[{"cos", "(", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
            "32805"}], ")"}]}], "5000000000000000"]}], "-", 
       FractionBox["113", "58044"]}]},
     {
      FractionBox[
       RowBox[{
        RowBox[{"119200714069641172375638", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"158856982758179383418895", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"12256442787481651944990", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"16333975310165236541475", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"24512885574963303889980", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"10592954879532108300", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"14117070218831700750", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        "115636334411063810121713"}], "3346417987500000000000000"], 
      FractionBox[
       RowBox[{
        RowBox[{"119200714069641172375638", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"12256442787481651944990", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"24512885574963303889980", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"10592954879532108300", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        "115636334411063810121713"}], "3346417987500000000000000"], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "24512885574963303889980"}], " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "-", 
        "115636334411063810121713"}], "3346417987500000000000000"], 
      FractionBox[
       RowBox[{
        RowBox[{"24512885574963303889980", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
        "115636334411063810121713"}], "3346417987500000000000000"], 
      RowBox[{
       FractionBox[
        RowBox[{"140267619", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
           "32805"}], ")"}]}], "5000000000000000"], "+", 
       FractionBox["113", "58044"]}]},
     {
      RowBox[{
       FractionBox[
        RowBox[{"140267619", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"173990", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "-", 
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "-", 
           "32805"}], ")"}]}], "5000000000000000"], "-", 
       FractionBox["113", "58044"]}], 
      RowBox[{
       FractionBox[
        RowBox[{"140267619", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "-", 
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "-", 
           "32805"}], ")"}]}], "5000000000000000"], "-", 
       FractionBox["113", "58044"]}], 
      RowBox[{
       RowBox[{"-", 
        FractionBox[
         RowBox[{"140267619", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"130556", " ", 
             RowBox[{"cos", "(", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
            "32805"}], ")"}]}], "5000000000000000"]}], "-", 
       FractionBox["113", "58044"]}], 
      RowBox[{
       FractionBox[
        RowBox[{"140267619", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"130556", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
           "32805"}], ")"}]}], "5000000000000000"], "+", 
       FractionBox["113", "58044"]}], 
      FractionBox["41604413054086349", "14511000000000000000"]}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5158680094562664`*^9, 3.5158703473109837`*^9, 3.5158703921535487`*^9, 
   3.5158729294486732`*^9, 3.5158864749772024`*^9, 3.5158865871881995`*^9, 
   3.51588728922186*^9, 3.515887379795619*^9, 3.5158889590781207`*^9, 
   3.515889944347827*^9, 3.515890134775964*^9, 3.5159481863404646`*^9, 
   3.5159487349050283`*^9, 3.51594882667999*^9, {3.5159532433023605`*^9, 
   3.515953268197785*^9}, 3.515953591417272*^9, 3.515972560966591*^9, 
   3.5159856799899564`*^9, 3.51670697445613*^9, 3.5167086602445517`*^9, 
   3.516709624769719*^9, 3.5167096625078773`*^9, 3.5167097182590666`*^9, 
   3.516711207249232*^9, 3.51671254400169*^9, 3.516731831199275*^9, 
   3.5167514890313177`*^9, 3.517679149640687*^9, 3.5176795830644774`*^9, 
   3.517679672396587*^9, 3.5176800021294465`*^9, 3.5176859157846885`*^9, 
   3.517697964091812*^9, 3.517698401714843*^9, 3.517698515767366*^9, 
   3.5190621430314283`*^9, 3.519482385662789*^9, {3.5194856738648634`*^9, 
   3.519485689053732*^9}, 3.5194879626507745`*^9, 3.519488001321986*^9, 
   3.5194881198177633`*^9, 3.5194884204709597`*^9, 3.5194907021061554`*^9, 
   3.5194909761138277`*^9, 3.5195655646986704`*^9, 3.5196479587499104`*^9, 
   3.519648250144577*^9, 3.525812498283947*^9, 3.525812588289095*^9, 
   3.5258134098700867`*^9, 3.5258244716187153`*^9, 3.527175474071156*^9, 
   3.52717551064102*^9, 3.5271784373234377`*^9, 3.5271786993805637`*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate Coriolis matrix", "Subsubsection",
 CellChangeTimes->{{3.504392477990152*^9, 3.504392495055983*^9}, 
   3.513012592090603*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[ScriptCapitalC]", "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"InertiaToCoriolis", "[", 
     RowBox[{"\[ScriptCapitalD]", ",", 
      RowBox[{"Flatten", "[", "q", "]"}], ",", 
      RowBox[{"Flatten", "[", "dq", "]"}]}], "]"}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{
  3.50439255557097*^9, {3.51295513827149*^9, 3.512955172679001*^9}, {
   3.5129552037364693`*^9, 3.512955231006535*^9}, {3.512955263739307*^9, 
   3.5129553328924294`*^9}, {3.512992901952466*^9, 3.512992920307856*^9}, {
   3.5129940359160023`*^9, 3.512994058469393*^9}, 3.5129940928271093`*^9, {
   3.512994138765036*^9, 3.5129941583899803`*^9}, {3.51301260617554*^9, 
   3.513012607311402*^9}, {3.513012641937742*^9, 3.513012652342953*^9}, {
   3.513012738154763*^9, 3.513012748062248*^9}, 3.513012780026892*^9, {
   3.51301313424288*^9, 3.513013136853251*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["\<\
calculate the potential energy and \[ScriptCapitalG] matrix\
\>", "Subsubsection",
 CellChangeTimes->{
  3.50439250607954*^9, {3.512994059885461*^9, 3.512994061579727*^9}, {
   3.513013138038511*^9, 3.513013138090329*^9}, 3.51301482840405*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"V", "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"g", " ", 
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], 
        RowBox[{"Length", "[", "mm", "]"}]], 
       RowBox[{
        RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
        " ", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]}]}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"\[ScriptCapitalG]", "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{"q", ",", "1"}], "}"}]], "V"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.504392530579445*^9, 3.504392568536552*^9}, {
  3.5052280833869843`*^9, 3.5052280856333885`*^9}, {3.5125114305562534`*^9, 
  3.512511430665453*^9}, {3.512994063656911*^9, 3.512994082683117*^9}, {
  3.513013142091812*^9, 3.513013228804931*^9}, {3.5158498432526364`*^9, 
  3.5158498449907355`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["\<\
calculate the \[ScriptCapitalE] matrix for impact and the guard\
\>", "Subsubsection",
 CellChangeTimes->{
  3.50439250607954*^9, 3.504539201882091*^9, {3.513013067545057*^9, 
   3.513013077660357*^9}, {3.513015048236977*^9, 3.513015054334556*^9}, {
   3.513531976155762*^9, 3.513531976917461*^9}}],

Cell["the position of swing foot", "Text",
 CellChangeTimes->{{3.50453921178862*^9, 3.504539216085734*^9}, {
  3.50453924758814*^9, 3.504539276779558*^9}, {3.513531967714079*^9, 
  3.513531967756608*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
   RowBox[{"RPToHomogeneous", "[", 
    RowBox[{
     RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.513015087195842*^9, 3.513015090509893*^9}, {
   3.5167083039851747`*^9, 3.5167083075683794`*^9}, 3.516708384001751*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"ForwardKinematics", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q5"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.513015094248981*^9, 3.513015103484043*^9}, {
  3.513531984839549*^9, 3.513531986761064*^9}, {3.5158587372202473`*^9, 
  3.5158587445476665`*^9}, {3.5158696556534233`*^9, 3.5158696577865453`*^9}, {
  3.5158872712662287`*^9, 3.5158872784422407`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[ScriptCapitalE]", "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "qe", "]"}], ",", "1"}], "}"}]], 
     RowBox[{
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], 
      "\[RightDoubleBracket]"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.497873623815133*^9, 3.497873662882368*^9}, {
   3.497874034989651*^9, 3.4978740419880514`*^9}, 3.504539395679452*^9, 
   3.505228103167819*^9, {3.513013081342501*^9, 3.51301311670339*^9}, {
   3.513015040561296*^9, 3.513015044303407*^9}, 3.513534878131503*^9, {
   3.513873767730934*^9, 3.513873767796698*^9}, {3.5158587475438375`*^9, 
   3.5158587475908403`*^9}, {3.516708313664728*^9, 3.516708314824795*^9}},
 FontSize->14],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"\[ScriptCapitalE]", "/.", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", ".2345"}], 
    ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", ".1894"}], 
    ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
     RowBox[{"-", ".293"}]}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", ".094"}], 
    ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
     RowBox[{"-", ".210"}]}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", ".923"}]}], 
   "}"}]}]], "Input",
 CellChangeTimes->{{3.5155049882169037`*^9, 3.5155049988325105`*^9}, {
  3.5155051086107893`*^9, 3.515505175018588*^9}, {3.515858751579068*^9, 
  3.5158587586594734`*^9}, {3.515862162917186*^9, 3.515862165572338*^9}, {
  3.5167096832590647`*^9, 3.516709690275466*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"1", "0", 
      RowBox[{"-", "0.09544747668836438`"}], "0.2430085338559884`", 
      "0.4810099636462693`", 
      RowBox[{"-", "0.4810099636462693`"}], 
      RowBox[{"-", "0.22007570983164088`"}]},
     {"0", "1", 
      RowBox[{"-", "0.44817198873510994`"}], 
      RowBox[{"-", "0.36731650452778486`"}], 
      RowBox[{"-", "0.25991633710807366`"}], "0.25991633710807366`", 
      "0.26954918353076013`"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.515505175604621*^9, 3.5155188251463304`*^9, 3.515521317029858*^9, {
   3.515540872145503*^9, 3.515540897429814*^9}, 3.515840733384139*^9, 
   3.5158416261726465`*^9, 3.5158498631087723`*^9, 3.515850130505066*^9, {
   3.515850563007804*^9, 3.515850574491461*^9}, 3.5158508014404416`*^9, 
   3.51585107888031*^9, 3.5158515678492775`*^9, 3.5158516326898913`*^9, 
   3.515852382120756*^9, 3.5158525678783813`*^9, 3.515852741417307*^9, 
   3.5158527835807185`*^9, 3.5158549102943597`*^9, 3.5158549832665334`*^9, 
   3.5158552103445215`*^9, 3.515855244393469*^9, 3.515859575183176*^9, 
   3.515859620954794*^9, 3.515860178313673*^9, 3.5158602243243046`*^9, 
   3.5158605047343435`*^9, 3.51586073930876*^9, 3.5158613183848815`*^9, 
   3.5158615173662624`*^9, 3.5158621766229696`*^9, 3.5158641789991765`*^9, 
   3.515864639911539*^9, 3.5158657551363263`*^9, 3.5158658127926245`*^9, 
   3.5158703501281447`*^9, 3.515870394776699*^9, 3.51587293219083*^9, 
   3.515886480686813*^9, 3.5158865927730093`*^9, 3.51588729502507*^9, 
   3.515887385630029*^9, 3.515888965006131*^9, 3.5158899504474373`*^9, 
   3.515890140859975*^9, 3.5159481920500746`*^9, 3.5159487407706385`*^9, 
   3.515948833762402*^9, 3.51595327476416*^9, 3.5159535976856303`*^9, 
   3.5159725662928953`*^9, 3.5159856849072375`*^9, 3.5167069803244658`*^9, {
   3.516709677388729*^9, 3.5167096906144853`*^9}, 3.516709723898389*^9, 
   3.516711212897555*^9, 3.516712549638012*^9, 3.5167318366315856`*^9, 
   3.5167514944416275`*^9, 3.517679155377015*^9, 3.5176795886397963`*^9, 
   3.517679678137915*^9, 3.5176800111259613`*^9, 3.5176859230441036`*^9, 
   3.517697971282223*^9, 3.5176984089132547`*^9, 3.5176985229927793`*^9, 
   3.5190621503058443`*^9, 3.5194823961493883`*^9, 3.5194856990663047`*^9, 
   3.5194880112395535`*^9, 3.519488129819336*^9, 3.5194884303375244`*^9, 
   3.5194907121317286`*^9, 3.5194909860853977`*^9, 3.5195655717170715`*^9, 
   3.5196479686374755`*^9, 3.5196482601101465`*^9, 3.5258125031802273`*^9, 
   3.525812593199376*^9, 3.525813415018381*^9, 3.525824477349043*^9, 
   3.527175484237624*^9, 3.527175520515585*^9, 3.527178447742915*^9, 
   3.5271787055293427`*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"h", "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
     "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"hdot", " ", "=", "  ", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", "h", "]"}]}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.516708318191987*^9, 3.516708318664014*^9}, {
  3.516754669099207*^9, 3.516754685899168*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<jpos_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"pos", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513876298225372*^9, 3.513876314895954*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<D_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalD]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.512953878469522*^9, 3.512953886446665*^9}, {
   3.512994085488056*^9, 3.512994090994364*^9}, 3.512994135460699*^9, {
   3.513013423442089*^9, 3.513013424564995*^9}, {3.513014073959661*^9, 
   3.513014078429472*^9}, 3.513014297088332*^9, {3.513015201746145*^9, 
   3.513015202045883*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<C_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalC]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.512953878469522*^9, 3.512953886446665*^9}, {
   3.512994085488056*^9, 3.512994090994364*^9}, 3.512994135460699*^9, {
   3.513013423442089*^9, 3.513013424564995*^9}, {3.513014073959661*^9, 
   3.513014076194914*^9}, 3.513014294914336*^9, {3.513015198946243*^9, 
   3.513015199252923*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<G_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalG]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.512953878469522*^9, 3.512953886446665*^9}, {
   3.512994085488056*^9, 3.512994090994364*^9}, 3.512994135460699*^9, {
   3.513013423442089*^9, 3.513013424564995*^9}, {3.513014073959661*^9, 
   3.513014076194914*^9}, {3.513014291511217*^9, 3.51301431273912*^9}, {
   3.513015194622436*^9, 3.513015194950777*^9}, {3.5158609614944687`*^9, 
   3.515860974401207*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<De_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalD]e", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.512953878469522*^9, 3.512953886446665*^9}, {
   3.512994085488056*^9, 3.512994090994364*^9}, 3.512994135460699*^9, {
   3.513013423442089*^9, 3.513013424564995*^9}, {3.513014073959661*^9, 
   3.513014078429472*^9}, 3.513014297088332*^9, {3.51301484393123*^9, 
   3.513014845159042*^9}, {3.513015197361516*^9, 3.513015197908276*^9}, {
   3.513887638252694*^9, 3.513887640636993*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<E_mat\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalE]", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.4978952481411505`*^9, 3.4978953056554403`*^9}, {
   3.504541084026764*^9, 3.504541092118987*^9}, {3.504545862009512*^9, 
   3.504545864143497*^9}, 3.504652479388712*^9, {3.5125119377519436`*^9, 
   3.512511937845544*^9}, {3.512525693637813*^9, 3.512525694152614*^9}, {
   3.5125257246194677`*^9, 3.512525725415069*^9}, {3.513015036076006*^9, 
   3.513015036343996*^9}, {3.51301512396632*^9, 3.513015127387151*^9}, {
   3.513015189396898*^9, 3.513015190520248*^9}, {3.513887472707003*^9, 
   3.51388747514813*^9}, {3.513887645297176*^9, 3.513887645992246*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_sca\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"h", "/.", "statesubs"}]}], "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.4978952481411505`*^9, 3.4978953056554403`*^9}, {
   3.504541084026764*^9, 3.504541092118987*^9}, {3.504545862009512*^9, 
   3.504545864143497*^9}, 3.504652479388712*^9, {3.5125119377519436`*^9, 
   3.512511937845544*^9}, {3.512525693637813*^9, 3.512525694152614*^9}, {
   3.5125257246194677`*^9, 3.512525725415069*^9}, {3.513015036076006*^9, 
   3.513015036343996*^9}, {3.51301512396632*^9, 3.513015127387151*^9}, {
   3.513015181146682*^9, 3.513015186905603*^9}, {3.513887467972675*^9, 
   3.513887469003708*^9}, {3.513887648118017*^9, 3.513887648448756*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_dot_mat\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"hdot", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.4978952481411505`*^9, 3.4978953056554403`*^9}, {
   3.504541084026764*^9, 3.504541092118987*^9}, {3.504545862009512*^9, 
   3.504545864143497*^9}, 3.504652479388712*^9, {3.5125119377519436`*^9, 
   3.512511937845544*^9}, {3.512525693637813*^9, 3.512525694152614*^9}, {
   3.5125257246194677`*^9, 3.512525725415069*^9}, {3.513015036076006*^9, 
   3.513015036343996*^9}, {3.51301512396632*^9, 3.513015127387151*^9}, {
   3.513015181146682*^9, 3.513015186905603*^9}, {3.513887467972675*^9, 
   3.513887469003708*^9}, {3.513887648118017*^9, 3.513887648448756*^9}, {
   3.5167541423880806`*^9, 3.5167541451912413`*^9}}]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Choosing the Outputs", "Section",
 CellChangeTimes->{{3.5206963594857893`*^9, 3.520696361509555*^9}, {
  3.5210630974288096`*^9, 3.521063101088019*^9}}],

Cell[CellGroupData[{

Cell["Torso Angle", "Subsection",
 CellChangeTimes->{{3.5206963594857893`*^9, 3.520696361509555*^9}}],

Cell[CellGroupData[{

Cell["Torso non-stance thigh angle", "Subsubsection",
 CellChangeTimes->{{3.5206955441956043`*^9, 3.520695554257166*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"theta4", " ", "=", " ", 
  RowBox[{
   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}]], "Input",
 CellChangeTimes->{{3.52069555679044*^9, 3.520695570667728*^9}, {
  3.520709652691082*^9, 3.520709653905396*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], TraditionalForm]], "Output",\

 CellChangeTimes->{
  3.520695764131291*^9, 3.5206960704143267`*^9, 3.520701511828219*^9, 
   3.52070276324645*^9, 3.520704376229435*^9, 3.520709988911413*^9, 
   3.520710126165258*^9, 3.5207159873928633`*^9, 3.5207163673295946`*^9, 
   3.520734724342556*^9, 3.5207352285723963`*^9, 3.5207876776993103`*^9, 
   3.5207888724366446`*^9, 3.520897233418949*^9, 3.521303721918645*^9, 
   3.521303841348476*^9, 3.5214860413581176`*^9, 3.521487911132063*^9, 
   3.522245114434749*^9, 3.522245679705081*^9, 3.522246789441554*^9, 
   3.522249894097908*^9, 3.5223307038987594`*^9, 3.522356607288306*^9, 
   3.5223569124348235`*^9, 3.5224296507504063`*^9, 3.5225102534533854`*^9, 
   3.522511865181571*^9, 3.5227081320715885`*^9, 3.522775152229655*^9, 
   3.5227768385264463`*^9, 3.5230285765032434`*^9, 3.52303063131769*^9, 
   3.523031330015735*^9, 3.523557264038102*^9, 3.5237248712974825`*^9, 
   3.523726563148251*^9, 3.5239837412959995`*^9, 3.5239849889183598`*^9, 
   3.5239893038161573`*^9, 3.5239903275177097`*^9, 3.5239920210955772`*^9, 
   3.523992132678959*^9, 3.5239948144663486`*^9, 3.523994924194625*^9, 
   3.523995035162972*^9, {3.524248079216264*^9, 3.524248097715322*^9}, 
   3.5242481371495776`*^9, 3.524248524179714*^9, 3.5242499899753895`*^9, 
   3.5244001088288093`*^9, 3.5244002935079145`*^9, 3.5244062652999444`*^9, 
   3.524406546119916*^9, 3.524406780779147*^9, 3.5244078606182146`*^9, 
   3.5244154472646675`*^9, 3.524415572327821*^9, 3.524415848654626*^9, 
   3.524415945790182*^9, 3.524417909157303*^9, 3.5244180349574795`*^9, 
   3.5244182237777443`*^9, 3.5244182682178063`*^9, 3.5244183895479765`*^9, 
   3.524418696910963*^9, 3.5244222197153735`*^9, 3.524422306928362*^9, 
   3.5244227483456097`*^9, 3.524423837994934*^9, 3.524442212810913*^9, 
   3.5244427382499666`*^9, 3.52444384502427*^9, 3.524445682151348*^9, 
   3.524445860306538*^9, 3.5244468643195024`*^9, 3.5245024263422694`*^9, 
   3.5245119400387764`*^9, 3.5245883948980503`*^9, 3.5250902827743287`*^9, 
   3.5250916908664265`*^9, 3.5250919477185745`*^9, 3.5250921857706423`*^9, 
   3.525092441109751*^9, 3.5250925668262205`*^9, 3.525092684983059*^9, 
   3.525092889373741*^9, 3.525093200641582*^9, 3.525098760867609*^9, 
   3.5254479844856815`*^9, {3.5254611873866744`*^9, 3.5254612120570855`*^9}, 
   3.525518261928259*^9, 3.5258244775240526`*^9, 3.527175706538025*^9, 
   3.527177461534525*^9, 3.52717871710386*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Torso Hip angle", "Subsubsection",
 CellChangeTimes->{3.520695578765766*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"TorsoHipAngle", "=", " ", 
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
   RowBox[{
    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
   RowBox[{
    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]}]], "Input",
 CellChangeTimes->{{3.5206945398251047`*^9, 3.520694570527237*^9}, {
  3.5206954481403933`*^9, 3.520695451085517*^9}, {3.520709696720999*^9, 
  3.520709699861195*^9}, {3.520710054090211*^9, 3.520710059923527*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
   RowBox[{
    SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
   RowBox[{
    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.520694571151453*^9, 3.520694952373599*^9, 3.5206957660064383`*^9, 
   3.520696070462152*^9, 3.52070151197847*^9, 3.520702763430723*^9, 
   3.520704376400757*^9, 3.520709988972275*^9, 3.520710060296135*^9, {
   3.520710105872642*^9, 3.520710126264868*^9}, 3.520715987418865*^9, 
   3.5207163673565965`*^9, 3.520734724394559*^9, 3.5207352285983973`*^9, 
   3.520787677729312*^9, 3.5207888724656467`*^9, 3.5208972334549513`*^9, 
   3.521303721937646*^9, 3.521303841368477*^9, 3.52148604139812*^9, 
   3.5214879111640644`*^9, 3.522245114468751*^9, 3.5222456797510834`*^9, 
   3.5222467894875565`*^9, 3.522249894149911*^9, 3.522330703975764*^9, 
   3.5223566073195057`*^9, 3.5223569124660234`*^9, 3.522429650756407*^9, 
   3.522510253512389*^9, 3.522511865247575*^9, 3.5227081321535935`*^9, 
   3.5227751522556562`*^9, 3.5227768385744495`*^9, 3.523028576571247*^9, 
   3.523030631357692*^9, 3.5230313300457373`*^9, 3.5235572641081057`*^9, 
   3.5237248713254843`*^9, 3.523726563161252*^9, 3.523983741387005*^9, 
   3.523984988935361*^9, 3.5239893038291583`*^9, 3.5239903275437117`*^9, 
   3.523992021113578*^9, 3.523992132713961*^9, 3.5239948144793496`*^9, 
   3.523994924217626*^9, 3.523995035175973*^9, {3.524248079275267*^9, 
   3.524248097740323*^9}, 3.5242481371835794`*^9, 3.524248524207716*^9, 
   3.5242499900343924`*^9, 3.5244001088488092`*^9, 3.5244002935269156`*^9, 
   3.5244062653189454`*^9, 3.524406546145918*^9, 3.524406780807149*^9, 
   3.524407860650216*^9, 3.524415447287669*^9, 3.524415572380824*^9, 
   3.524415848673627*^9, 3.5244159458051825`*^9, 3.5244179091773033`*^9, 
   3.52441803497748*^9, 3.5244182238177443`*^9, 3.5244182682378063`*^9, 
   3.5244183895679765`*^9, 3.524418696910963*^9, 3.5244222197303743`*^9, 
   3.5244223069543633`*^9, 3.52442274834961*^9, 3.524423837996934*^9, 
   3.524442212823914*^9, 3.524442738266968*^9, 3.524443845048272*^9, 
   3.52444568218035*^9, 3.5244458603305397`*^9, 3.524446864348504*^9, 
   3.5245024263942723`*^9, 3.5245119400767784`*^9, 3.5245883949160514`*^9, 
   3.52509028279133*^9, 3.5250916908954277`*^9, 3.5250919477485743`*^9, 
   3.5250921857996445`*^9, 3.5250924411317525`*^9, 3.5250925668512225`*^9, 
   3.525092685024061*^9, 3.525092889393741*^9, 3.5250932007045856`*^9, 
   3.5250987608996105`*^9, 3.525447984506683*^9, {3.5254611874066753`*^9, 
   3.5254612120770864`*^9}, 3.525518261954261*^9, 3.525824477542054*^9, 
   3.527175709548682*^9, 3.527177461593964*^9, 3.527178717257341*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Torso Non-stance Slope", "Subsubsection",
 CellChangeTimes->{{3.520695590202653*^9, 3.5206955960981827`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"nstorso", " ", "=", " ", 
  FractionBox[
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
     "\[LeftDoubleBracket]", 
     RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
     "\[LeftDoubleBracket]", 
     RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
     "\[LeftDoubleBracket]", 
     RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
     "\[LeftDoubleBracket]", 
     RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]]}]], "Input",
 CellChangeTimes->{{3.520694584709077*^9, 3.520694656511403*^9}, {
   3.520695454209565*^9, 3.520695460889751*^9}, 3.520695602271639*^9, 
   3.520701580119817*^9, {3.520709707312653*^9, 3.5207097132186213`*^9}}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{
    FractionBox[
     RowBox[{
      RowBox[{"250000", " ", 
       RowBox[{
        SubscriptBox["p", "x"], "(", "t", ")"}]}], "+", 
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"-", 
          RowBox[{"685", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "-", 
         RowBox[{"514", " ", 
          RowBox[{"sin", "(", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
         RowBox[{"514", " ", 
          RowBox[{"sin", "(", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
         RowBox[{"685", " ", 
          RowBox[{"sin", "(", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
        ")"}]}]}], "250000"], "+", 
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"685", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
       ")"}]}], "250000"]}], 
   RowBox[{
    FractionBox[
     RowBox[{
      RowBox[{"250000", " ", 
       RowBox[{
        SubscriptBox["p", "z"], "(", "t", ")"}]}], "+", 
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"685", " ", 
          RowBox[{"cos", "(", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
         RowBox[{"514", " ", 
          RowBox[{"cos", "(", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
         RowBox[{"514", " ", 
          RowBox[{"cos", "(", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
         RowBox[{"685", " ", 
          RowBox[{"cos", "(", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
        ")"}]}]}], "250000"], "-", 
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"685", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
       ")"}]}], "250000"]}]], TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5207015804968557`*^9, 3.5207027635115833`*^9, 3.520704376509199*^9, 
   3.5207099893242617`*^9, {3.520710100904756*^9, 3.520710126354644*^9}, 
   3.5207159874918694`*^9, 3.520716367434601*^9, 3.520734724477564*^9, 
   3.520735228674402*^9, 3.520787677808316*^9, 3.520788872537651*^9, 
   3.520897233537956*^9, 3.52130372200465*^9, 3.5213038414434814`*^9, 
   3.5214860414851246`*^9, 3.521487911230068*^9, 3.5222451145347548`*^9, 
   3.5222456798330884`*^9, 3.5222467895685616`*^9, 3.522249894225916*^9, 
   3.5223307040777693`*^9, 3.5223566073819065`*^9, 3.5223569125440235`*^9, 
   3.52242965080641*^9, 3.522510253596394*^9, 3.522511865361582*^9, 
   3.5227081328856354`*^9, 3.5227751522776575`*^9, 3.5227768387004566`*^9, 
   3.523028576690254*^9, 3.5230306314666967`*^9, 3.5230313301217413`*^9, 
   3.5235572642211123`*^9, 3.523724871355486*^9, 3.5237265631792526`*^9, 
   3.5239837414480085`*^9, 3.523984988954362*^9, 3.523989303844159*^9, 
   3.5239903275737133`*^9, 3.5239920211365795`*^9, 3.5239921327469635`*^9, 
   3.5239948144973507`*^9, 3.5239949242416277`*^9, 3.5239950351929736`*^9, {
   3.524248079309269*^9, 3.5242480977743254`*^9}, 3.524248137211581*^9, 
   3.5242485242377176`*^9, 3.5242499901213975`*^9, 3.5244001088588095`*^9, 
   3.524400293543916*^9, 3.5244062653409467`*^9, 3.5244065461769185`*^9, 
   3.524406780838151*^9, 3.5244078606892185`*^9, 3.5244154473106704`*^9, 
   3.524415572396825*^9, 3.524415848692628*^9, 3.524415945816183*^9, 
   3.5244179091973033`*^9, 3.5244180350074797`*^9, 3.5244182238377447`*^9, 
   3.524418268257807*^9, 3.524418389577976*^9, 3.524418696910963*^9, 
   3.5244222197523756`*^9, 3.5244223069813647`*^9, 3.5244227483566103`*^9, 
   3.524423837999934*^9, 3.524442212838915*^9, 3.5244427382879686`*^9, 
   3.524443845073273*^9, 3.5244456822113514`*^9, 3.524445860359541*^9, 
   3.5244468644275084`*^9, 3.524502426493278*^9, 3.5245119401787844`*^9, 
   3.5245883949290524`*^9, 3.525090282809331*^9, 3.5250916909234295`*^9, 
   3.525091947778575*^9, 3.525092185832646*^9, 3.5250924411567535`*^9, 
   3.5250925668772235`*^9, 3.5250926850670633`*^9, 3.525092889413741*^9, 
   3.525093200766589*^9, 3.5250987609326124`*^9, 3.5254479845306845`*^9, {
   3.5254611874306765`*^9, 3.525461212100088*^9}, 3.525518261977262*^9, 
   3.5258244775640554`*^9, 3.527175710891769*^9, 3.527177461712614*^9, 
   3.527178717371438*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   RowBox[{"nstorsoFile", " ", "=", " ", 
    RowBox[{
     RowBox[{"\"\<nstorsoFun.m\>\"", "\n", 
      FormBox["\"\<nstorsoFun.m\>\"",
       TraditionalForm], "\n", 
      RowBox[{"Write", "[", 
       RowBox[{"nstorsoFile", ",", 
        RowBox[{
        "OutputForm", "[", "\"\<y = <*ToMatlab[nstorso/.statesubs/.p0]*>\>\"",
          "]"}]}], "]"}], "\n", 
      RowBox[{"Close", "[", "nstorsoFile", "]"}], "\n", 
      FormBox[
       RowBox[{
        StyleBox[
         RowBox[{"General", "::", "openx"}], "MessageName"], ":", " ", 
        "\"\<\[NoBreak]\\!\\(TraditionalForm\\`\\\"nstorsoFun.m\\\"\\)\
\[NoBreak] is not open. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/openx\\\", ButtonNote -> \
\\\"General::openx\\\"]\\)\>\""}],
       TraditionalForm], "\n", 
      FormBox[
       RowBox[{"Close", "[", "\"\<nstorsoFun.m\>\"", "]"}],
       TraditionalForm], "\n", 
      RowBox[{"FilePrint", "[", "nstorsoFile", "]"}], "\n", "y"}], " ", "=", 
     " ", 
     RowBox[{
      RowBox[{"<", 
       RowBox[{"*", 
        RowBox[{"ToMatlab", "[", 
         RowBox[{
          RowBox[{"nstorso", "/.", "statesubs"}], "/.", "p0"}], "]"}], "*"}], 
       ">", "\n", 
       RowBox[{
        RowBox[{"Splice", "[", "\"\<nstorsoFun.m\>\"", "]"}], "\n", 
        FormBox["\"\<nstorsoFun.m\>\"",
         TraditionalForm], "\n", 
        RowBox[{"FilePrint", "[", "\"\<nstorsoFun.m\>\"", "]"}], "\n", 
        "y"}]}], " ", "=", " ", 
      RowBox[{
       RowBox[{"<", 
        RowBox[{"*", 
         RowBox[{"ToMatlab", "[", 
          RowBox[{
           RowBox[{"nstorso", "/.", "statesubs"}], "/.", "p0"}], "]"}], "*"}],
         ">", "\n", 
        RowBox[{"?", 
         RowBox[{"WriteMatlab", "\n", 
          
          StyleBox[
           "\!\(\*\"\\\"\"\)\!\(\*\"WriteMatlab\"\)\!\(\*\"[\"\)\!\(\*\"expr\"\
\)\!\(\*\",\"\)\!\(\*\" \"\)\!\(\*\"file\"\)\!\(\*\"]\"\)\!\(\*\"  \"\)\!\(\*\
\"or\"\)\!\(\*\"  \
\"\)\!\(\*\"WriteMatlab\"\)\!\(\*\"[\"\)\!\(\*\"expr\"\)\!\(\*\",\"\)\!\(\*\" \
\"\)\!\(\*\"file\"\)\!\(\*\",\"\)\!\(\*\" \"\)\!\(\*\"name\"\)\!\(\*\"]\"\)\!\
\(\*\" \"\)\!\(\*\"Writes\"\)\!\(\*\" \"\)\!\(\*\"the\"\)\!\(\*\"  \"\)\!\(\*\
\"expr\"\)\!\(\*\"  \"\)\!\(\*\"in\"\)\!\(\*\" \"\)\!\(\*\"matlab\"\)\!\(\*\" \
\"\)\!\(\*\"form\"\)\!\(\*\" \"\)\!\(\*\"into\"\)\!\(\*\" \"\)\!\(\*\"the\"\)\
\!\(\*\" \"\)\!\(\*\"given\"\)\!\(\*\" \
\"\)\!\(\*\"file\"\)\!\(\*\".\"\)\!\(\*\" \"\)\!\(\*\"The\"\)\!\(\*\" \
\"\)\!\(\*\"second\"\)\!\(\*\" \"\)\!\(\*\"form\"\)\!\(\*\" \"\)\!\(\*\"makes\
\"\)\!\(\*\" \"\)\!\(\*\"this\"\)\!\(\*\" \"\)\!\(\*\"an\"\)\!\(\*\" \"\)\!\(\
\*\"assignment\"\)\!\(\*\" \"\)\!\(\*\"into\"\)\!\(\*\" \"\)\!\(\*\"the\"\)\!\
\(\*\" \"\)\!\(\*\"variable\"\)\!\(\*\"  \
\"\)\!\(\*\"name\"\)\!\(\*\".\"\)\!\(\*\"\\\\n\"\)\!\(\*\"Example\"\)\!\(\*\":\
\"\)\!\(\*\" \"\)\!\(\*\"f\"\)\!\(\*\" \"\)\!\(\*\"=\"\)\!\(\*\" \
\"\)\!\(\*\"OpenWrite\"\)\!\(\*\"[\"\)\!\(\*\"\\\\\\\"\"\)\!\(\*\"file\"\)\!\(\
\*\".\"\)\!\(\*\"m\"\)\!\(\*\"\\\\\\\"\"\)\!\(\*\"]\"\)\!\(\*\";\"\)\!\(\*\" \
\"\)\!\(\*\"WriteMatlab\"\)\!\(\*\"[\"\)\!\(\*\"Cos\"\)\!\(\*\"[\"\)\!\(\*\"x\
\"\)\!\(\*\"]\"\)\!\(\*\"-\"\)\!\(\*\"x\"\)\!\(\*\",\"\)\!\(\*\" \
\"\)\!\(\*\"f\"\)\!\(\*\",\"\)\!\(\*\" \
\"\)\!\(\*\"y\"\)\!\(\*\"]\"\)\!\(\*\";\"\)\!\(\*\" \"\)\!\(\*\"Close\"\)\!\(\
\*\"[\"\)\!\(\*\"f\"\)\!\(\*\"]\"\)\!\(\*\";\"\)\!\(\*\"\\\\n\"\)\!\(\*\"The\"\
\)\!\(\*\" \"\)\!\(\*\"file\"\)\!\(\*\" \"\)\!\(\*\"argument\"\)\!\(\*\" \"\)\
\!\(\*\"can\"\)\!\(\*\" \"\)\!\(\*\"also\"\)\!\(\*\" \"\)\!\(\*\"be\"\)\!\(\*\
\" \"\)\!\(\*\"a\"\)\!\(\*\" \"\)\!\(\*\"String\"\)\!\(\*\" \
\"\)\!\(\*\"that\"\)\!\(\*\" \"\)\!\(\*\"gives\"\)\!\(\*\" \
\"\)\!\(\*\"the\"\)\!\(\*\" \"\)\!\(\*\"name\"\)\!\(\*\" \"\)\!\(\*\"of\"\)\!\
\(\*\" \"\)\!\(\*\"the\"\)\!\(\*\" \"\)\!\(\*\"file\"\)\!\(\*\":\"\)\!\(\*\" \
\"\)\!\(\*\"WriteMatlab\"\)\!\(\*\"[\"\)\!\(\*\"Cos\"\)\!\(\*\"[\"\)\!\(\*\"x\
\"\)\!\(\*\"]\"\)\!\(\*\"-\"\)\!\(\*\"x\"\)\!\(\*\",\"\)\!\(\*\" \"\)\!\(\*\"\
\\\\\\\"\"\)\!\(\*\"file\"\)\!\(\*\".\"\)\!\(\*\"m\"\)\!\(\*\"\\\\\\\"\"\)\!\(\
\*\",\"\)\!\(\*\" \"\)\!\(\*\"y\"\)\!\(\*\"]\"\)\!\(\*\";\"\)\!\(\*\" \
\"\)\!\(\*\"achieves\"\)\!\(\*\" \"\)\!\(\*\"the\"\)\!\(\*\" \"\)\!\(\*\"same\
\"\)\!\(\*\" \"\)\!\(\*\"result\"\)\!\(\*\" \"\)\!\(\*\"as\"\)\!\(\*\" \"\)\!\
\(\*\"the\"\)\!\(\*\" \"\)\!\(\*\"previous\"\)\!\(\*\" \
\"\)\!\(\*\"example\"\)\!\(\*\" \"\)\!\(\*\"(\"\)\!\(\*\"but\"\)\!\(\*\" \"\)\
\!\(\*\"this\"\)\!\(\*\" \"\)\!\(\*\"limits\"\)\!\(\*\" \"\)\!\(\*\"one\"\)\!\
\(\*\" \"\)\!\(\*\"expression\"\)\!\(\*\" \"\)\!\(\*\"per\"\)\!\(\*\" \
\"\)\!\(\*\"file\"\)\!\(\*\")\"\)\!\(\*\".\"\)\!\(\*\"\\\\n\"\)\!\(\*\"See\"\)\
\!\(\*\" \"\)\!\(\*\"also\"\)\!\(\*\"  \"\)\!\(\*\"ToMatlab\"\)\!\(\*\"  \"\)\
\!\(\*\"and\"\)\!\(\*\"  \"\)\!\(\*\"PrintMatlab\"\)\!\(\*\".\"\)\!\(\*\"\\\"\
\"\)", "MSG"], "\n", "nstorsoFile1"}]}]}], " ", "=", " ", 
       "\"\<nstorsoFun.m\>\""}]}]}]}], ";", "\[IndentingNewLine]", 
   RowBox[{"WriteMatlab", "[", 
    RowBox[{
     RowBox[{"ToMatlab", "[", 
      RowBox[{
       RowBox[{"nstorso", "/.", "statesubs"}], "/.", "p0"}], "]"}], ",", " ", 
     "nstorsoFile1", ",", " ", "Ynstorso"}], "]"}], ";", 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{"Close", "[", "nstorsoFile1", "]"}], "\[IndentingNewLine]", 
      RowBox[{"FilePrint", "[", "nstorsoFile", "]"}], "\n", 
      FormBox[
       RowBox[{
        StyleBox[
         RowBox[{"General", "::", "openx"}], "MessageName"], ":", " ", 
        "\"\<\[NoBreak]\\!\\(TraditionalForm\\`\\\"nstorsoFun.m\\\"\\)\
\[NoBreak] is not open. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/openx\\\", ButtonNote -> \
\\\"General::openx\\\"]\\)\>\""}],
       TraditionalForm], "\n", 
      FormBox[
       RowBox[{"Close", "[", "\"\<nstorsoFun.m\>\"", "]"}],
       TraditionalForm], "\n", 
      RowBox[{
       RowBox[{"Ynstorso", "=", 
        RowBox[{
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{
              RowBox[{"(", 
               RowBox[{
                RowBox[{"-", "127"}], "/", "250000"}], ")"}], "."}], "*", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{"685.", "*", "cos", 
                RowBox[{"(", 
                 RowBox[{"x", 
                  RowBox[{"(", "1", ")"}]}], ")"}]}], " ", "+", " ", 
               RowBox[{"514.", "*", "cos", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"x", 
                   RowBox[{"(", "1", ")"}]}], " ", "+", " ", 
                  RowBox[{"x", 
                   RowBox[{"(", "2", ")"}]}]}], ")"}]}]}], ")"}]}], " ", "+", 
            " ", 
            RowBox[{
             RowBox[{
              RowBox[{"(", " ", 
               RowBox[{
                RowBox[{"...", "\n", "   ", "..."}], "\n", "  ", 
                RowBox[{"127", "/", "250000"}]}], ")"}], "."}], "*", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{
                 RowBox[{
                  RowBox[{
                   RowBox[{"685.", "*", "cos", 
                    RowBox[{"(", 
                    RowBox[{"x", 
                    RowBox[{"(", "1", ")"}]}], ")"}]}], " ", "+", " ", 
                   RowBox[{"514.", "*", "cos", 
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"x", 
                    RowBox[{"(", "1", ")"}]}], " ", "+", " ", 
                    RowBox[{"x", 
                    RowBox[{"(", "2", ")"}]}]}], ")"}]}], " ", "+", " ", 
                   RowBox[{
                    RowBox[{
                    RowBox[{"(", 
                    RowBox[{"-", "514"}], ")"}], "."}], "*"}]}], " ", "..."}],
                  "\n", "   ", "..."}], "\n", "  ", "cos", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"x", 
                   RowBox[{"(", "1", ")"}]}], " ", "+", " ", 
                  RowBox[{"x", 
                   RowBox[{"(", "2", ")"}]}], " ", "+", " ", 
                  RowBox[{"x", 
                   RowBox[{"(", "3", ")"}]}], " ", "+", " ", 
                  RowBox[{
                   RowBox[{
                    RowBox[{"(", 
                    RowBox[{"-", "1"}], ")"}], "."}], "*", "x", 
                   RowBox[{"(", "4", ")"}]}]}], ")"}]}], " ", "+", " ", 
               RowBox[{
                RowBox[{
                 RowBox[{"(", 
                  RowBox[{"-", "685"}], ")"}], "."}], "*", "cos", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{
                   RowBox[{
                    RowBox[{
                    RowBox[{
                    RowBox[{"x", 
                    RowBox[{"(", "1", ")"}]}], " ", "+", " ", 
                    RowBox[{"x", 
                    RowBox[{"(", "2", ")"}]}], " ", "+"}], "  ", "..."}], 
                    "\n", "   ", "..."}], "\n", "  ", "x", 
                   RowBox[{"(", "3", ")"}]}], " ", "+", " ", 
                  RowBox[{
                   RowBox[{
                    RowBox[{"(", 
                    RowBox[{"-", "1"}], ")"}], "."}], "*", "x", 
                   RowBox[{"(", "4", ")"}]}], " ", "+", " ", 
                  RowBox[{
                   RowBox[{
                    RowBox[{"(", 
                    RowBox[{"-", "1"}], ")"}], "."}], "*", "x", 
                   RowBox[{"(", "5", ")"}]}]}], ")"}]}]}], ")"}]}]}], ")"}], 
          ".", 
          RowBox[{"^", 
           RowBox[{"(", 
            RowBox[{"-", "1"}], ")"}]}], "."}], "*", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{
             RowBox[{"(", 
              RowBox[{"127", "/", "250000"}], ")"}], "."}], "*", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"685.", "*", "sin", 
               RowBox[{"(", " ", 
                RowBox[{
                 RowBox[{"...", "\n", "  ", "..."}], "\n", "   ", "x", 
                 RowBox[{"(", "1", ")"}]}], ")"}]}], " ", "+", " ", 
              RowBox[{"514.", "*", "sin", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"x", 
                  RowBox[{"(", "1", ")"}]}], " ", "+", " ", 
                 RowBox[{"x", 
                  RowBox[{"(", "2", ")"}]}]}], ")"}]}]}], ")"}]}], " ", "+", 
           " ", 
           RowBox[{
            RowBox[{
             RowBox[{"(", 
              RowBox[{"127", "/", "250000"}], ")"}], "."}], "*", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{
                RowBox[{
                 RowBox[{
                  RowBox[{"(", 
                   RowBox[{"-", "685"}], ")"}], "."}], "*", "sin", 
                 RowBox[{"(", 
                  RowBox[{"x", 
                   RowBox[{"(", "1", ")"}]}], ")"}]}], "   ", "..."}], "\n", 
               "  ", "..."}], "\n", "  ", "+", " ", 
              RowBox[{
               RowBox[{
                RowBox[{"(", 
                 RowBox[{"-", "514"}], ")"}], "."}], "*", "sin", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"x", 
                  RowBox[{"(", "1", ")"}]}], " ", "+", " ", 
                 RowBox[{"x", 
                  RowBox[{"(", "2", ")"}]}]}], ")"}]}], " ", "+", " ", 
              RowBox[{"514.", "*", "sin", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{
                  RowBox[{
                   RowBox[{
                    RowBox[{"x", 
                    RowBox[{"(", "1", ")"}]}], " ", "+", " ", 
                    RowBox[{"x", 
                    RowBox[{"(", "2", ")"}]}], " ", "+", " ", 
                    RowBox[{"x", 
                    RowBox[{"(", "3", ")"}]}], " ", "+", " ", 
                    RowBox[{
                    RowBox[{
                    RowBox[{"(", 
                    RowBox[{"-", "1"}], ")"}], "."}], "*"}]}], "  ", "..."}], 
                  "\n", "  ", "..."}], "\n", "  ", "x", 
                 RowBox[{"(", "4", ")"}]}], ")"}]}], " ", "+", " ", 
              RowBox[{"685.", "*", "sin", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"x", 
                  RowBox[{"(", "1", ")"}]}], " ", "+", " ", 
                 RowBox[{"x", 
                  RowBox[{"(", "2", ")"}]}], " ", "+", " ", 
                 RowBox[{"x", 
                  RowBox[{"(", "3", ")"}]}], " ", "+", " ", 
                 RowBox[{
                  RowBox[{
                   RowBox[{"(", 
                    RowBox[{"-", "1"}], ")"}], "."}], "*", "x", 
                  RowBox[{"(", "4", ")"}]}], " ", "+", " ", 
                 RowBox[{
                  RowBox[{
                   RowBox[{"(", 
                    RowBox[{"-", "1"}], ")"}], "."}], "*", "x", 
                  RowBox[{"(", "5", ")"}]}]}], ")"}]}]}], ")"}]}]}], 
          ")"}]}]}], ";"}]}], "  ", "..."}], "\n", "  ", "..."}], "\n", "  ", 
   "\n", ";", "\n", 
   RowBox[{
    RowBox[{"ToMatlab", "[", 
     RowBox[{
      RowBox[{"nstorso", "/.", "statesubs"}], "/.", "p0"}], "]"}], "\n", 
    FormBox[
     "\"\<((-127/250000).*(685.*cos(x(1)) + 514.*cos(x(1) + x(2))) + ( ...\\n \
 127/250000).*(685.*cos(x(1)) + 514.*cos(x(1) + x(2)) + (-514).* ...\\n  \
cos(x(1) + x(2) + x(3) + (-1).*x(4)) + (-685).*cos(x(1) + x(2) +  ...\\n  \
x(3) + (-1).*x(4) + (-1).*x(5)))).^(-1).*((127/250000).*(685.*sin( ...\\n  \
x(1)) + 514.*sin(x(1) + x(2))) + (127/250000).*((-685).*sin(x(1))  ...\\n  + \
(-514).*sin(x(1) + x(2)) + 514.*sin(x(1) + x(2) + x(3) + (-1).* ...\\n  x(4)) \
+ 685.*sin(x(1) + x(2) + x(3) + (-1).*x(4) + (-1).*x(5)))); ...\\n  \\n\>\"",
     TraditionalForm]}]}], "*)"}]], "Input",
 CellChangeTimes->{{3.527175782152529*^9, 3.5271758054214573`*^9}, {
  3.527175938200865*^9, 3.527175983432848*^9}, {3.527176804001871*^9, 
  3.527176844679709*^9}},
 CellTags->"Info3527158436-4200434"]
}, Open  ]],

Cell[CellGroupData[{

Cell["Linearized torso non-stance slope angle", "Subsubsection",
 CellChangeTimes->{{3.520695613570808*^9, 3.520695621834859*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"LinearNStorsoPre", "=", 
   RowBox[{"Normal", "[", 
    RowBox[{"Series", "[", 
     RowBox[{
      RowBox[{
       FractionBox[
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", "p0"}],
       ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], ",", "0", ",", "1"}],
        "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], ",", "0", ",", "1"}],
        "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], ",", "0", ",", "1"}],
        "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], ",", "0", ",", "1"}],
        "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], ",", "0", ",", "1"}],
        "}"}]}], "]"}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.520694721538422*^9, 3.520694772324471*^9}, {
   3.520694828943734*^9, 3.520694856785536*^9}, {3.5206954051023703`*^9, 
   3.520695405486321*^9}, {3.520695467271062*^9, 3.520695479996497*^9}, {
   3.520695645850556*^9, 3.5206956514828167`*^9}, 3.520696341953876*^9, {
   3.5207097177181797`*^9, 3.520709725854377*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{"pt1", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearNStorsoPre", ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"pt2", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearNStorsoPre", ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"pt3", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearNStorsoPre", ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"pt4", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearNStorsoPre", ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"pt5", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearNStorsoPre", ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}]}], "Input",
 CellChangeTimes->{{3.520694958957471*^9, 3.520694986818905*^9}, {
  3.520695045380724*^9, 3.520695045568749*^9}, {3.520695156033687*^9, 
  3.520695206561957*^9}, {3.520695239884231*^9, 3.520695264903411*^9}, {
  3.520695349094801*^9, 3.5206954158398037`*^9}, {3.52069549049266*^9, 
  3.520695498572467*^9}, {3.520701593044855*^9, 3.520701597133*^9}, {
  3.520709734561612*^9, 3.5207097470894547`*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"-", 
   RowBox[{
    SubscriptBox["\[Theta]", "1"], "(", "t", ")"}]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.520695206789549*^9, 3.520695772818528*^9, 3.5206960705465403`*^9, 
   3.520701513189907*^9, {3.520701566318336*^9, 3.52070160852745*^9}, 
   3.52070276475139*^9, 3.520704377500181*^9, 3.520709989772189*^9, 
   3.520710127466627*^9, 3.5207159880018983`*^9, 3.5207163679206285`*^9, 
   3.5207347250515966`*^9, 3.5207352292054324`*^9, 3.5207876783613477`*^9, 
   3.520788873015678*^9, 3.5208972344900107`*^9, 3.521303722498678*^9, 
   3.5213038419895124`*^9, 3.5214860423141727`*^9, 3.521487911738097*^9, 
   3.5222451149967813`*^9, 3.522245680301115*^9, 3.5222467900915914`*^9, 
   3.5222498951339674`*^9, 3.5223307062298927`*^9, 3.522356608005907*^9, 
   3.5223569130120244`*^9, 3.5224296513154387`*^9, 3.522510254645454*^9, 
   3.5225118660046186`*^9, 3.522708133772686*^9, 3.52277515267168*^9, 
   3.522776839567506*^9, 3.523028577328291*^9, 3.5230306324727507`*^9, 
   3.5230313309867907`*^9, 3.5235572649931564`*^9, 3.5237248717705097`*^9, 
   3.523726563563275*^9, 3.523983741860032*^9, 3.5239849893343835`*^9, 
   3.523989304221181*^9, 3.5239903279627357`*^9, 3.523992021524602*^9, 
   3.523992133145986*^9, 3.523994814878372*^9, 3.52399492463665*^9, 
   3.523995035572995*^9, 3.5242481375846024`*^9, 3.5242485246287403`*^9, 
   3.524249991144456*^9, 3.52440010924881*^9, 3.5244002939149375`*^9, 
   3.524406265734969*^9, 3.5244065465669394`*^9, 3.5244067812331734`*^9, 
   3.524407861091242*^9, 3.524415447744695*^9, 3.5244155727708464`*^9, 
   3.52441584907165*^9, 3.5244159462182064`*^9, 3.524417909577304*^9, 
   3.52441803538748*^9, 3.524418224217745*^9, 3.5244182682778063`*^9, 
   3.5244183899579773`*^9, 3.524418697293974*^9, 3.5244222201984015`*^9, 
   3.5244223074193897`*^9, 3.524422748758633*^9, 3.524423838402957*^9, 
   3.5244422132729397`*^9, 3.5244427386699905`*^9, 3.5244438455192986`*^9, 
   3.524445682602374*^9, 3.5244458607555637`*^9, 3.524446864915537*^9, 
   3.5245024275813403`*^9, 3.52451194080682*^9, 3.524588395361077*^9, 
   3.5250902832443557`*^9, 3.525091691317452*^9, 3.5250919481685753`*^9, 
   3.5250921862346687`*^9, 3.525092441546776*^9, 3.5250925672672462`*^9, 
   3.525092685476087*^9, 3.525092889798752*^9, 3.5250932013866243`*^9, 
   3.5250987616146517`*^9, 3.5254479850207124`*^9, {3.525461187853701*^9, 
   3.5254612125191116`*^9}, 3.525518263469347*^9, 3.525824478373101*^9, 
   3.527177462146447*^9, 3.527178717775655*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"-", 
   RowBox[{
    SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.520695206789549*^9, 3.520695772818528*^9, 3.5206960705465403`*^9, 
   3.520701513189907*^9, {3.520701566318336*^9, 3.52070160852745*^9}, 
   3.52070276475139*^9, 3.520704377500181*^9, 3.520709989772189*^9, 
   3.520710127466627*^9, 3.5207159880018983`*^9, 3.5207163679206285`*^9, 
   3.5207347250515966`*^9, 3.5207352292054324`*^9, 3.5207876783613477`*^9, 
   3.520788873015678*^9, 3.5208972344900107`*^9, 3.521303722498678*^9, 
   3.5213038419895124`*^9, 3.5214860423141727`*^9, 3.521487911738097*^9, 
   3.5222451149967813`*^9, 3.522245680301115*^9, 3.5222467900915914`*^9, 
   3.5222498951339674`*^9, 3.5223307062298927`*^9, 3.522356608005907*^9, 
   3.5223569130120244`*^9, 3.5224296513154387`*^9, 3.522510254645454*^9, 
   3.5225118660046186`*^9, 3.522708133772686*^9, 3.52277515267168*^9, 
   3.522776839567506*^9, 3.523028577328291*^9, 3.5230306324727507`*^9, 
   3.5230313309867907`*^9, 3.5235572649931564`*^9, 3.5237248717705097`*^9, 
   3.523726563563275*^9, 3.523983741860032*^9, 3.5239849893343835`*^9, 
   3.523989304221181*^9, 3.5239903279627357`*^9, 3.523992021524602*^9, 
   3.523992133145986*^9, 3.523994814878372*^9, 3.52399492463665*^9, 
   3.523995035572995*^9, 3.5242481375846024`*^9, 3.5242485246287403`*^9, 
   3.524249991144456*^9, 3.52440010924881*^9, 3.5244002939149375`*^9, 
   3.524406265734969*^9, 3.5244065465669394`*^9, 3.5244067812331734`*^9, 
   3.524407861091242*^9, 3.524415447744695*^9, 3.5244155727708464`*^9, 
   3.52441584907165*^9, 3.5244159462182064`*^9, 3.524417909577304*^9, 
   3.52441803538748*^9, 3.524418224217745*^9, 3.5244182682778063`*^9, 
   3.5244183899579773`*^9, 3.524418697293974*^9, 3.5244222201984015`*^9, 
   3.5244223074193897`*^9, 3.524422748758633*^9, 3.524423838402957*^9, 
   3.5244422132729397`*^9, 3.5244427386699905`*^9, 3.5244438455192986`*^9, 
   3.524445682602374*^9, 3.5244458607555637`*^9, 3.524446864915537*^9, 
   3.5245024275813403`*^9, 3.52451194080682*^9, 3.524588395361077*^9, 
   3.5250902832443557`*^9, 3.525091691317452*^9, 3.5250919481685753`*^9, 
   3.5250921862346687`*^9, 3.525092441546776*^9, 3.5250925672672462`*^9, 
   3.525092685476087*^9, 3.525092889798752*^9, 3.5250932013866243`*^9, 
   3.5250987616146517`*^9, 3.5254479850207124`*^9, {3.525461187853701*^9, 
   3.5254612125191116`*^9}, 3.525518263469347*^9, 3.525824478373101*^9, 
   3.527177462146447*^9, 3.527178717777461*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"-", 
   RowBox[{
    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.520695206789549*^9, 3.520695772818528*^9, 3.5206960705465403`*^9, 
   3.520701513189907*^9, {3.520701566318336*^9, 3.52070160852745*^9}, 
   3.52070276475139*^9, 3.520704377500181*^9, 3.520709989772189*^9, 
   3.520710127466627*^9, 3.5207159880018983`*^9, 3.5207163679206285`*^9, 
   3.5207347250515966`*^9, 3.5207352292054324`*^9, 3.5207876783613477`*^9, 
   3.520788873015678*^9, 3.5208972344900107`*^9, 3.521303722498678*^9, 
   3.5213038419895124`*^9, 3.5214860423141727`*^9, 3.521487911738097*^9, 
   3.5222451149967813`*^9, 3.522245680301115*^9, 3.5222467900915914`*^9, 
   3.5222498951339674`*^9, 3.5223307062298927`*^9, 3.522356608005907*^9, 
   3.5223569130120244`*^9, 3.5224296513154387`*^9, 3.522510254645454*^9, 
   3.5225118660046186`*^9, 3.522708133772686*^9, 3.52277515267168*^9, 
   3.522776839567506*^9, 3.523028577328291*^9, 3.5230306324727507`*^9, 
   3.5230313309867907`*^9, 3.5235572649931564`*^9, 3.5237248717705097`*^9, 
   3.523726563563275*^9, 3.523983741860032*^9, 3.5239849893343835`*^9, 
   3.523989304221181*^9, 3.5239903279627357`*^9, 3.523992021524602*^9, 
   3.523992133145986*^9, 3.523994814878372*^9, 3.52399492463665*^9, 
   3.523995035572995*^9, 3.5242481375846024`*^9, 3.5242485246287403`*^9, 
   3.524249991144456*^9, 3.52440010924881*^9, 3.5244002939149375`*^9, 
   3.524406265734969*^9, 3.5244065465669394`*^9, 3.5244067812331734`*^9, 
   3.524407861091242*^9, 3.524415447744695*^9, 3.5244155727708464`*^9, 
   3.52441584907165*^9, 3.5244159462182064`*^9, 3.524417909577304*^9, 
   3.52441803538748*^9, 3.524418224217745*^9, 3.5244182682778063`*^9, 
   3.5244183899579773`*^9, 3.524418697293974*^9, 3.5244222201984015`*^9, 
   3.5244223074193897`*^9, 3.524422748758633*^9, 3.524423838402957*^9, 
   3.5244422132729397`*^9, 3.5244427386699905`*^9, 3.5244438455192986`*^9, 
   3.524445682602374*^9, 3.5244458607555637`*^9, 3.524446864915537*^9, 
   3.5245024275813403`*^9, 3.52451194080682*^9, 3.524588395361077*^9, 
   3.5250902832443557`*^9, 3.525091691317452*^9, 3.5250919481685753`*^9, 
   3.5250921862346687`*^9, 3.525092441546776*^9, 3.5250925672672462`*^9, 
   3.525092685476087*^9, 3.525092889798752*^9, 3.5250932013866243`*^9, 
   3.5250987616146517`*^9, 3.5254479850207124`*^9, {3.525461187853701*^9, 
   3.5254612125191116`*^9}, 3.525518263469347*^9, 3.525824478373101*^9, 
   3.527177462146447*^9, 3.527178717778604*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{
   SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], TraditionalForm]], "Output",\

 CellChangeTimes->{
  3.520695206789549*^9, 3.520695772818528*^9, 3.5206960705465403`*^9, 
   3.520701513189907*^9, {3.520701566318336*^9, 3.52070160852745*^9}, 
   3.52070276475139*^9, 3.520704377500181*^9, 3.520709989772189*^9, 
   3.520710127466627*^9, 3.5207159880018983`*^9, 3.5207163679206285`*^9, 
   3.5207347250515966`*^9, 3.5207352292054324`*^9, 3.5207876783613477`*^9, 
   3.520788873015678*^9, 3.5208972344900107`*^9, 3.521303722498678*^9, 
   3.5213038419895124`*^9, 3.5214860423141727`*^9, 3.521487911738097*^9, 
   3.5222451149967813`*^9, 3.522245680301115*^9, 3.5222467900915914`*^9, 
   3.5222498951339674`*^9, 3.5223307062298927`*^9, 3.522356608005907*^9, 
   3.5223569130120244`*^9, 3.5224296513154387`*^9, 3.522510254645454*^9, 
   3.5225118660046186`*^9, 3.522708133772686*^9, 3.52277515267168*^9, 
   3.522776839567506*^9, 3.523028577328291*^9, 3.5230306324727507`*^9, 
   3.5230313309867907`*^9, 3.5235572649931564`*^9, 3.5237248717705097`*^9, 
   3.523726563563275*^9, 3.523983741860032*^9, 3.5239849893343835`*^9, 
   3.523989304221181*^9, 3.5239903279627357`*^9, 3.523992021524602*^9, 
   3.523992133145986*^9, 3.523994814878372*^9, 3.52399492463665*^9, 
   3.523995035572995*^9, 3.5242481375846024`*^9, 3.5242485246287403`*^9, 
   3.524249991144456*^9, 3.52440010924881*^9, 3.5244002939149375`*^9, 
   3.524406265734969*^9, 3.5244065465669394`*^9, 3.5244067812331734`*^9, 
   3.524407861091242*^9, 3.524415447744695*^9, 3.5244155727708464`*^9, 
   3.52441584907165*^9, 3.5244159462182064`*^9, 3.524417909577304*^9, 
   3.52441803538748*^9, 3.524418224217745*^9, 3.5244182682778063`*^9, 
   3.5244183899579773`*^9, 3.524418697293974*^9, 3.5244222201984015`*^9, 
   3.5244223074193897`*^9, 3.524422748758633*^9, 3.524423838402957*^9, 
   3.5244422132729397`*^9, 3.5244427386699905`*^9, 3.5244438455192986`*^9, 
   3.524445682602374*^9, 3.5244458607555637`*^9, 3.524446864915537*^9, 
   3.5245024275813403`*^9, 3.52451194080682*^9, 3.524588395361077*^9, 
   3.5250902832443557`*^9, 3.525091691317452*^9, 3.5250919481685753`*^9, 
   3.5250921862346687`*^9, 3.525092441546776*^9, 3.5250925672672462`*^9, 
   3.525092685476087*^9, 3.525092889798752*^9, 3.5250932013866243`*^9, 
   3.5250987616146517`*^9, 3.5254479850207124`*^9, {3.525461187853701*^9, 
   3.5254612125191116`*^9}, 3.525518263469347*^9, 3.525824478373101*^9, 
   3.527177462146447*^9, 3.5271787177796288`*^9}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{"685", " ", 
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], "1199"], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.520695206789549*^9, 3.520695772818528*^9, 3.5206960705465403`*^9, 
   3.520701513189907*^9, {3.520701566318336*^9, 3.52070160852745*^9}, 
   3.52070276475139*^9, 3.520704377500181*^9, 3.520709989772189*^9, 
   3.520710127466627*^9, 3.5207159880018983`*^9, 3.5207163679206285`*^9, 
   3.5207347250515966`*^9, 3.5207352292054324`*^9, 3.5207876783613477`*^9, 
   3.520788873015678*^9, 3.5208972344900107`*^9, 3.521303722498678*^9, 
   3.5213038419895124`*^9, 3.5214860423141727`*^9, 3.521487911738097*^9, 
   3.5222451149967813`*^9, 3.522245680301115*^9, 3.5222467900915914`*^9, 
   3.5222498951339674`*^9, 3.5223307062298927`*^9, 3.522356608005907*^9, 
   3.5223569130120244`*^9, 3.5224296513154387`*^9, 3.522510254645454*^9, 
   3.5225118660046186`*^9, 3.522708133772686*^9, 3.52277515267168*^9, 
   3.522776839567506*^9, 3.523028577328291*^9, 3.5230306324727507`*^9, 
   3.5230313309867907`*^9, 3.5235572649931564`*^9, 3.5237248717705097`*^9, 
   3.523726563563275*^9, 3.523983741860032*^9, 3.5239849893343835`*^9, 
   3.523989304221181*^9, 3.5239903279627357`*^9, 3.523992021524602*^9, 
   3.523992133145986*^9, 3.523994814878372*^9, 3.52399492463665*^9, 
   3.523995035572995*^9, 3.5242481375846024`*^9, 3.5242485246287403`*^9, 
   3.524249991144456*^9, 3.52440010924881*^9, 3.5244002939149375`*^9, 
   3.524406265734969*^9, 3.5244065465669394`*^9, 3.5244067812331734`*^9, 
   3.524407861091242*^9, 3.524415447744695*^9, 3.5244155727708464`*^9, 
   3.52441584907165*^9, 3.5244159462182064`*^9, 3.524417909577304*^9, 
   3.52441803538748*^9, 3.524418224217745*^9, 3.5244182682778063`*^9, 
   3.5244183899579773`*^9, 3.524418697293974*^9, 3.5244222201984015`*^9, 
   3.5244223074193897`*^9, 3.524422748758633*^9, 3.524423838402957*^9, 
   3.5244422132729397`*^9, 3.5244427386699905`*^9, 3.5244438455192986`*^9, 
   3.524445682602374*^9, 3.5244458607555637`*^9, 3.524446864915537*^9, 
   3.5245024275813403`*^9, 3.52451194080682*^9, 3.524588395361077*^9, 
   3.5250902832443557`*^9, 3.525091691317452*^9, 3.5250919481685753`*^9, 
   3.5250921862346687`*^9, 3.525092441546776*^9, 3.5250925672672462`*^9, 
   3.525092685476087*^9, 3.525092889798752*^9, 3.5250932013866243`*^9, 
   3.5250987616146517`*^9, 3.5254479850207124`*^9, {3.525461187853701*^9, 
   3.5254612125191116`*^9}, 3.525518263469347*^9, 3.525824478373101*^9, 
   3.527177462146447*^9, 3.527178717780661*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"LinearNStorso", " ", "=", " ", 
  RowBox[{"pt1", "+", "pt2", "+", "pt3", "+", "pt4", "+", "pt5"}]}]], "Input",\

 CellChangeTimes->{{3.520695397142486*^9, 3.5206954329665327`*^9}, 
   3.52069551209443*^9, 3.520696017044744*^9, 3.520696336723299*^9, {
   3.520709753026821*^9, 3.520709755763522*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   RowBox[{"-", 
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "(", "t", ")"}]}], "-", 
   RowBox[{
    SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "-", 
   RowBox[{
    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "+", 
   RowBox[{
    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "+", 
   FractionBox[
    RowBox[{"685", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], "1199"]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.520696017386157*^9, 3.5206960706285067`*^9, 3.5206963375706873`*^9, 
   3.520701513422699*^9, 3.520701613568445*^9, 3.520702764889917*^9, 
   3.520704377691307*^9, 3.5207099899789753`*^9, 3.5207101275363607`*^9, 
   3.5207159880359*^9, 3.5207163679536304`*^9, 3.5207347250895987`*^9, 
   3.5207352292374344`*^9, 3.52078767839735*^9, 3.52078887304768*^9, 
   3.520897234535013*^9, 3.5213037225266795`*^9, 3.5213038420255146`*^9, 
   3.5214860423561745`*^9, 3.521487911773099*^9, 3.5222451150327835`*^9, 
   3.522245680356118*^9, 3.5222467901425943`*^9, 3.52224989518297*^9, 
   3.522330706272895*^9, 3.5223566080371075`*^9, 3.5223569130588245`*^9, 
   3.5224296513284397`*^9, 3.5225102546934566`*^9, 3.522511866084623*^9, 
   3.5227081338136883`*^9, 3.5227751526996813`*^9, 3.5227768396095085`*^9, 
   3.5230285773922944`*^9, 3.5230306324937515`*^9, 3.5230313313188095`*^9, 
   3.523557265025158*^9, 3.5237248718075123`*^9, 3.5237265635802755`*^9, 
   3.523983741886033*^9, 3.523984989362385*^9, 3.523989304240182*^9, 
   3.5239903279927373`*^9, 3.5239920215466027`*^9, 3.5239921331739874`*^9, 
   3.5239948149033737`*^9, 3.5239949246626515`*^9, 3.5239950355929966`*^9, 
   3.5242481376106043`*^9, 3.524248524659742*^9, 3.5242499911864586`*^9, 
   3.5244001092588096`*^9, 3.524400293931938*^9, 3.5244062657569704`*^9, 
   3.524406546598941*^9, 3.524406781264175*^9, 3.524407861129244*^9, 
   3.5244154477676964`*^9, 3.5244155727968483`*^9, 3.5244158491006517`*^9, 
   3.524415946233207*^9, 3.524417909607304*^9, 3.52441803541748*^9, 
   3.5244182242377453`*^9, 3.5244182682978067`*^9, 3.524418389987977*^9, 
   3.524418697317975*^9, 3.5244222202264028`*^9, 3.5244223074513917`*^9, 
   3.5244227487646337`*^9, 3.5244238384079576`*^9, 3.5244422132909408`*^9, 
   3.524442738692992*^9, 3.5244438455483007`*^9, 3.524445682635376*^9, 
   3.524445860782565*^9, 3.524446864956539*^9, 3.5245024276283426`*^9, 
   3.524511940835822*^9, 3.5245883953850784`*^9, 3.5250902832773576`*^9, 
   3.525091691349454*^9, 3.5250919482085752`*^9, 3.525092186288672*^9, 
   3.5250924415717773`*^9, 3.5250925672962475`*^9, 3.5250926855200896`*^9, 
   3.5250928898217525`*^9, 3.525093201445628*^9, 3.5250987616456532`*^9, 
   3.525447985046714*^9, {3.525461187882703*^9, 3.5254612125421133`*^9}, 
   3.525518263499349*^9, 3.5258244783981028`*^9, 3.527177462230262*^9, 
   3.52717871785013*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Torso stance slope", "Subsubsection",
 CellChangeTimes->{{3.520695525211995*^9, 3.5206955357072773`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"storso", " ", "=", " ", 
  FractionBox[
   RowBox[{
    RowBox[{
     SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
    "\[LeftDoubleBracket]", 
    RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], 
   RowBox[{
    RowBox[{
     SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
    "\[LeftDoubleBracket]", 
    RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]]}]], "Input",
 CellChangeTimes->{{3.520694295474862*^9, 3.520694315353154*^9}, {
   3.5206957919864693`*^9, 3.520695811213437*^9}, 3.520696060761739*^9, 
   3.520701530245644*^9, {3.520709762529334*^9, 3.520709767330147*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"-", 
   FractionBox[
    RowBox[{
     RowBox[{"685", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
     RowBox[{"514", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
    RowBox[{
     RowBox[{"685", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
     RowBox[{"514", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}]]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.520696070691399*^9, {3.520701513520948*^9, 3.520701530587276*^9}, 
   3.520702764959996*^9, 3.520704377805355*^9, 3.520709990048913*^9, 
   3.520710127613027*^9, 3.5207159880679026`*^9, 3.5207163679856324`*^9, 
   3.5207347251296005`*^9, 3.520735229271436*^9, 3.520787678434352*^9, 
   3.5207888730816817`*^9, 3.520897234581016*^9, 3.521303722557681*^9, 
   3.5213038420605164`*^9, 3.5214860423991776`*^9, 3.521487911808101*^9, 
   3.5222451150677853`*^9, 3.5222456805281277`*^9, 3.5222467902125983`*^9, 
   3.522249895229973*^9, 3.522330706317898*^9, 3.5223566080995073`*^9, 
   3.522356913090025*^9, 3.5224296513474407`*^9, 3.522510254725458*^9, 
   3.5225118661616273`*^9, 3.522708133851691*^9, 3.5227751527156825`*^9, 
   3.5227768397125144`*^9, 3.5230285775923057`*^9, 3.523030632517753*^9, 
   3.523031331355812*^9, 3.52355726505716*^9, 3.523724871839514*^9, 
   3.523726563595277*^9, 3.5239837419040346`*^9, 3.523984989383386*^9, 
   3.523989304254183*^9, 3.5239903280187387`*^9, 3.523992021562604*^9, 
   3.5239921331909885`*^9, 3.5239948149453764`*^9, 3.5239949246856527`*^9, 
   3.5239950356089973`*^9, 3.524248137628605*^9, 3.5242485246887436`*^9, 
   3.524249991231461*^9, 3.5244001092788095`*^9, 3.524400293943939*^9, 
   3.5244062657739716`*^9, 3.5244065466259427`*^9, 3.524406781291177*^9, 
   3.5244078611622458`*^9, 3.5244154477826977`*^9, 3.524415572818849*^9, 
   3.524415849124653*^9, 3.524415946244208*^9, 3.5244179096373043`*^9, 
   3.52441803544748*^9, 3.524418224267745*^9, 3.5244182683178067`*^9, 
   3.5244183900079775`*^9, 3.524418697319975*^9, 3.524422220242404*^9, 
   3.5244223074793935`*^9, 3.524422748769634*^9, 3.5244238384109573`*^9, 
   3.5244422133049417`*^9, 3.524442738710993*^9, 3.5244438455733023`*^9, 
   3.5244456826643777`*^9, 3.524445860805567*^9, 3.524446864993541*^9, 
   3.5245024276713457`*^9, 3.5245119408678236`*^9, 3.524588395402079*^9, 
   3.5250902832953587`*^9, 3.5250916913764553`*^9, 3.525091948238575*^9, 
   3.5250921863176737`*^9, 3.5250924415917788`*^9, 3.5250925673222494`*^9, 
   3.525092685562092*^9, 3.525092889839754*^9, 3.52509320148463*^9, 
   3.5250987616686544`*^9, 3.525447985064715*^9, {3.525461187902704*^9, 
   3.5254612125671144`*^9}, 3.5255182635203505`*^9, 3.525824478419104*^9, 
   3.527177462302165*^9, 3.527178717905053*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Linearized Torso stance slope", "Subsubsection",
 CellChangeTimes->{{3.520696029480558*^9, 3.5206960356234713`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"LinearStorsoPre", "=", 
  RowBox[{"Normal", "[", 
   RowBox[{"Series", "[", 
    RowBox[{
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]], "/.", "p0"}], 
     ",", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], ",", "0", ",", "1"}], 
      "}"}], ",", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], ",", "0", ",", "1"}], 
      "}"}], ",", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], ",", "0", ",", "1"}], 
      "}"}], ",", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], ",", "0", ",", "1"}], 
      "}"}], ",", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], ",", "0", ",", "1"}], 
      "}"}]}], "]"}], "]"}]}]], "Input",
 CellChangeTimes->{{3.52069593672266*^9, 3.520695944809325*^9}, 
   3.5206959783951063`*^9, 3.520696064492345*^9, 3.520696267522843*^9, 
   3.520701533829845*^9, {3.5207097803735313`*^9, 3.520709787883758*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   RowBox[{"-", 
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "(", "t", ")"}]}], "-", 
   FractionBox[
    RowBox[{"514", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], "1199"]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.520695978660975*^9, 3.520696070737742*^9, 3.5206962686319323`*^9, {
   3.520701513690805*^9, 3.520701534190587*^9}, 3.520702765137992*^9, 
   3.520704377960699*^9, 3.5207099901133223`*^9, 3.520710127722888*^9, 
   3.5207159881729083`*^9, 3.520716368015634*^9, 3.5207347251696033`*^9, 
   3.520735229305438*^9, 3.5207876784733543`*^9, 3.5207888731136837`*^9, 
   3.5208972346240187`*^9, 3.5213037225896835`*^9, 3.521303842097519*^9, 
   3.5214860424431796`*^9, 3.521487911842103*^9, 3.5222451151027875`*^9, 
   3.522245680654135*^9, 3.522246790338606*^9, 3.522249895274976*^9, 
   3.5223307063599005`*^9, 3.5223566081151075`*^9, 3.522356913121225*^9, 
   3.522429651362441*^9, 3.5225102547574606`*^9, 3.522511866235632*^9, 
   3.5227081339066935`*^9, 3.522775152744684*^9, 3.5227768397545166`*^9, 
   3.52302857766831*^9, 3.5230306325427547`*^9, 3.5230313316288276`*^9, 
   3.5235572650901623`*^9, 3.5237248718745155`*^9, 3.523726563614278*^9, 
   3.5239837419250355`*^9, 3.523984989405388*^9, 3.523989304273184*^9, 
   3.5239903280497403`*^9, 3.523992021585605*^9, 3.52399213321399*^9, 
   3.523994814977378*^9, 3.5239949247136545`*^9, 3.5239950356299987`*^9, 
   3.524248137647606*^9, 3.524248524720745*^9, 3.524249991279464*^9, 
   3.5244001092988095`*^9, 3.52440029395994*^9, 3.5244062657929726`*^9, 
   3.5244065466579447`*^9, 3.5244067813231783`*^9, 3.524407861202248*^9, 
   3.5244154478016987`*^9, 3.52441557283785*^9, 3.5244158491456537`*^9, 
   3.5244159462592087`*^9, 3.524417909657304*^9, 3.52441803547748*^9, 
   3.524418224287745*^9, 3.5244182683278065`*^9, 3.524418390037977*^9, 
   3.524418697326976*^9, 3.524422220261405*^9, 3.524422307522396*^9, 
   3.5244227487806344`*^9, 3.5244238384179583`*^9, 3.5244422133209424`*^9, 
   3.5244427387359943`*^9, 3.5244438456013036`*^9, 3.52444568269938*^9, 
   3.5244458608345685`*^9, 3.5244468650375433`*^9, 3.5245024277153482`*^9, 
   3.5245119408968253`*^9, 3.5245883954190803`*^9, 3.52509028332236*^9, 
   3.525091691410457*^9, 3.525091948288575*^9, 3.525092186353676*^9, 
   3.5250924416197805`*^9, 3.525092567353251*^9, 3.5250926856090946`*^9, 
   3.5250928898667555`*^9, 3.525093201530633*^9, 3.525098761707657*^9, 
   3.525447985087716*^9, {3.5254611879247055`*^9, 3.5254612125901155`*^9}, 
   3.525518263550352*^9, 3.525824478439105*^9, 3.527177462384432*^9, 
   3.5271787180154552`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{"pt1", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearStorsoPre", ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"pt2", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearStorsoPre", ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"pt3", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearStorsoPre", 
     StyleBox[",",
      FontWeight->"Plain"], 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"pt4", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearStorsoPre", ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"pt5", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearStorsoPre", ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}]}], "Input",
 CellChangeTimes->{{3.520695955439434*^9, 3.520695962402815*^9}, {
  3.520696273472088*^9, 3.52069627758632*^9}, {3.520709795342469*^9, 
  3.520709817219591*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"-", 
   RowBox[{
    SubscriptBox["\[Theta]", "1"], "(", "t", ")"}]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5206959629626102`*^9, 3.520696070780498*^9, 3.520696278715569*^9, {
   3.520701513696095*^9, 3.5207015403271513`*^9}, 3.520702765231444*^9, 
   3.52070437802107*^9, 3.5207099901878777`*^9, 3.520710127740677*^9, 
   3.5207159882149105`*^9, 3.520716368042636*^9, 3.5207347252676086`*^9, 
   3.5207352293354397`*^9, 3.5207876785073557`*^9, 3.520788873138685*^9, 
   3.5208972346630206`*^9, 3.521303722618685*^9, 3.5213038422055244`*^9, 
   3.521486042482182*^9, 3.5214879118661046`*^9, 3.522245115133789*^9, 
   3.522245680701138*^9, 3.522246790380608*^9, 3.5222498953139777`*^9, 
   3.5223307063999023`*^9, 3.522356608146308*^9, 3.522356913152425*^9, 
   3.522429651370442*^9, 3.5225102547904625`*^9, 3.5225118663016353`*^9, 
   3.5227081342657146`*^9, 3.5227751527686853`*^9, 3.5227768397965193`*^9, 
   3.523028577736314*^9, 3.5230306325597553`*^9, 3.5230313316658297`*^9, 
   3.5235572651211634`*^9, 3.523724871912518*^9, 3.523726563631279*^9, 
   3.5239837419440365`*^9, 3.5239849894283886`*^9, 3.5239893042871847`*^9, 
   3.5239903280797424`*^9, 3.523992021605606*^9, 3.523992133239991*^9, 
   3.5239948150013795`*^9, 3.523994924738656*^9, 3.5239950356489997`*^9, 
   3.524248137665607*^9, 3.5242485247497473`*^9, 3.5242499913234663`*^9, 
   3.5244001093188095`*^9, 3.5244002939729404`*^9, 3.5244062658109736`*^9, 
   3.524406546684946*^9, 3.52440678135618*^9, 3.5244078612372503`*^9, 
   3.5244154478237*^9, 3.5244155728548512`*^9, 3.5244158491636553`*^9, 
   3.5244159462702093`*^9, 3.524417909687304*^9, 3.5244180354974804`*^9, 
   3.524418224307745*^9, 3.524418268347807*^9, 3.524418390067977*^9, 
   3.5244186973299756`*^9, 3.524422220282406*^9, 3.5244223075543976`*^9, 
   3.524422748788635*^9, 3.5244238384219584`*^9, 3.5244422133339434`*^9, 
   3.5244427387569957`*^9, 3.524443845627305*^9, 3.5244456827323813`*^9, 
   3.5244458608565693`*^9, 3.5244468650735455`*^9, 3.5245024277533503`*^9, 
   3.524511940918826*^9, 3.5245883954350815`*^9, 3.525090283351362*^9, 
   3.5250916914404593`*^9, 3.525091948318575*^9, 3.525092186390678*^9, 
   3.5250924416737833`*^9, 3.5250925673782525`*^9, 3.5250926856510973`*^9, 
   3.525092889896757*^9, 3.5250932015706353`*^9, 3.5250987617346582`*^9, 
   3.5254479851077175`*^9, {3.525461187947706*^9, 3.5254612126121173`*^9}, 
   3.525518263574353*^9, 3.525824478456106*^9, 3.527177462454658*^9, 
   3.527178718070786*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"-", 
   FractionBox[
    RowBox[{"514", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], "1199"]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5206959629626102`*^9, 3.520696070780498*^9, 3.520696278715569*^9, {
   3.520701513696095*^9, 3.5207015403271513`*^9}, 3.520702765231444*^9, 
   3.52070437802107*^9, 3.5207099901878777`*^9, 3.520710127740677*^9, 
   3.5207159882149105`*^9, 3.520716368042636*^9, 3.5207347252676086`*^9, 
   3.5207352293354397`*^9, 3.5207876785073557`*^9, 3.520788873138685*^9, 
   3.5208972346630206`*^9, 3.521303722618685*^9, 3.5213038422055244`*^9, 
   3.521486042482182*^9, 3.5214879118661046`*^9, 3.522245115133789*^9, 
   3.522245680701138*^9, 3.522246790380608*^9, 3.5222498953139777`*^9, 
   3.5223307063999023`*^9, 3.522356608146308*^9, 3.522356913152425*^9, 
   3.522429651370442*^9, 3.5225102547904625`*^9, 3.5225118663016353`*^9, 
   3.5227081342657146`*^9, 3.5227751527686853`*^9, 3.5227768397965193`*^9, 
   3.523028577736314*^9, 3.5230306325597553`*^9, 3.5230313316658297`*^9, 
   3.5235572651211634`*^9, 3.523724871912518*^9, 3.523726563631279*^9, 
   3.5239837419440365`*^9, 3.5239849894283886`*^9, 3.5239893042871847`*^9, 
   3.5239903280797424`*^9, 3.523992021605606*^9, 3.523992133239991*^9, 
   3.5239948150013795`*^9, 3.523994924738656*^9, 3.5239950356489997`*^9, 
   3.524248137665607*^9, 3.5242485247497473`*^9, 3.5242499913234663`*^9, 
   3.5244001093188095`*^9, 3.5244002939729404`*^9, 3.5244062658109736`*^9, 
   3.524406546684946*^9, 3.52440678135618*^9, 3.5244078612372503`*^9, 
   3.5244154478237*^9, 3.5244155728548512`*^9, 3.5244158491636553`*^9, 
   3.5244159462702093`*^9, 3.524417909687304*^9, 3.5244180354974804`*^9, 
   3.524418224307745*^9, 3.524418268347807*^9, 3.524418390067977*^9, 
   3.5244186973299756`*^9, 3.524422220282406*^9, 3.5244223075543976`*^9, 
   3.524422748788635*^9, 3.5244238384219584`*^9, 3.5244422133339434`*^9, 
   3.5244427387569957`*^9, 3.524443845627305*^9, 3.5244456827323813`*^9, 
   3.5244458608565693`*^9, 3.5244468650735455`*^9, 3.5245024277533503`*^9, 
   3.524511940918826*^9, 3.5245883954350815`*^9, 3.525090283351362*^9, 
   3.5250916914404593`*^9, 3.525091948318575*^9, 3.525092186390678*^9, 
   3.5250924416737833`*^9, 3.5250925673782525`*^9, 3.5250926856510973`*^9, 
   3.525092889896757*^9, 3.5250932015706353`*^9, 3.5250987617346582`*^9, 
   3.5254479851077175`*^9, {3.525461187947706*^9, 3.5254612126121173`*^9}, 
   3.525518263574353*^9, 3.525824478456106*^9, 3.527177462454658*^9, 
   3.527178718080799*^9}],

Cell[BoxData[
 FormBox["0", TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5206959629626102`*^9, 3.520696070780498*^9, 3.520696278715569*^9, {
   3.520701513696095*^9, 3.5207015403271513`*^9}, 3.520702765231444*^9, 
   3.52070437802107*^9, 3.5207099901878777`*^9, 3.520710127740677*^9, 
   3.5207159882149105`*^9, 3.520716368042636*^9, 3.5207347252676086`*^9, 
   3.5207352293354397`*^9, 3.5207876785073557`*^9, 3.520788873138685*^9, 
   3.5208972346630206`*^9, 3.521303722618685*^9, 3.5213038422055244`*^9, 
   3.521486042482182*^9, 3.5214879118661046`*^9, 3.522245115133789*^9, 
   3.522245680701138*^9, 3.522246790380608*^9, 3.5222498953139777`*^9, 
   3.5223307063999023`*^9, 3.522356608146308*^9, 3.522356913152425*^9, 
   3.522429651370442*^9, 3.5225102547904625`*^9, 3.5225118663016353`*^9, 
   3.5227081342657146`*^9, 3.5227751527686853`*^9, 3.5227768397965193`*^9, 
   3.523028577736314*^9, 3.5230306325597553`*^9, 3.5230313316658297`*^9, 
   3.5235572651211634`*^9, 3.523724871912518*^9, 3.523726563631279*^9, 
   3.5239837419440365`*^9, 3.5239849894283886`*^9, 3.5239893042871847`*^9, 
   3.5239903280797424`*^9, 3.523992021605606*^9, 3.523992133239991*^9, 
   3.5239948150013795`*^9, 3.523994924738656*^9, 3.5239950356489997`*^9, 
   3.524248137665607*^9, 3.5242485247497473`*^9, 3.5242499913234663`*^9, 
   3.5244001093188095`*^9, 3.5244002939729404`*^9, 3.5244062658109736`*^9, 
   3.524406546684946*^9, 3.52440678135618*^9, 3.5244078612372503`*^9, 
   3.5244154478237*^9, 3.5244155728548512`*^9, 3.5244158491636553`*^9, 
   3.5244159462702093`*^9, 3.524417909687304*^9, 3.5244180354974804`*^9, 
   3.524418224307745*^9, 3.524418268347807*^9, 3.524418390067977*^9, 
   3.5244186973299756`*^9, 3.524422220282406*^9, 3.5244223075543976`*^9, 
   3.524422748788635*^9, 3.5244238384219584`*^9, 3.5244422133339434`*^9, 
   3.5244427387569957`*^9, 3.524443845627305*^9, 3.5244456827323813`*^9, 
   3.5244458608565693`*^9, 3.5244468650735455`*^9, 3.5245024277533503`*^9, 
   3.524511940918826*^9, 3.5245883954350815`*^9, 3.525090283351362*^9, 
   3.5250916914404593`*^9, 3.525091948318575*^9, 3.525092186390678*^9, 
   3.5250924416737833`*^9, 3.5250925673782525`*^9, 3.5250926856510973`*^9, 
   3.525092889896757*^9, 3.5250932015706353`*^9, 3.5250987617346582`*^9, 
   3.5254479851077175`*^9, {3.525461187947706*^9, 3.5254612126121173`*^9}, 
   3.525518263574353*^9, 3.525824478456106*^9, 3.527177462454658*^9, 
   3.527178718082965*^9}],

Cell[BoxData[
 FormBox["0", TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5206959629626102`*^9, 3.520696070780498*^9, 3.520696278715569*^9, {
   3.520701513696095*^9, 3.5207015403271513`*^9}, 3.520702765231444*^9, 
   3.52070437802107*^9, 3.5207099901878777`*^9, 3.520710127740677*^9, 
   3.5207159882149105`*^9, 3.520716368042636*^9, 3.5207347252676086`*^9, 
   3.5207352293354397`*^9, 3.5207876785073557`*^9, 3.520788873138685*^9, 
   3.5208972346630206`*^9, 3.521303722618685*^9, 3.5213038422055244`*^9, 
   3.521486042482182*^9, 3.5214879118661046`*^9, 3.522245115133789*^9, 
   3.522245680701138*^9, 3.522246790380608*^9, 3.5222498953139777`*^9, 
   3.5223307063999023`*^9, 3.522356608146308*^9, 3.522356913152425*^9, 
   3.522429651370442*^9, 3.5225102547904625`*^9, 3.5225118663016353`*^9, 
   3.5227081342657146`*^9, 3.5227751527686853`*^9, 3.5227768397965193`*^9, 
   3.523028577736314*^9, 3.5230306325597553`*^9, 3.5230313316658297`*^9, 
   3.5235572651211634`*^9, 3.523724871912518*^9, 3.523726563631279*^9, 
   3.5239837419440365`*^9, 3.5239849894283886`*^9, 3.5239893042871847`*^9, 
   3.5239903280797424`*^9, 3.523992021605606*^9, 3.523992133239991*^9, 
   3.5239948150013795`*^9, 3.523994924738656*^9, 3.5239950356489997`*^9, 
   3.524248137665607*^9, 3.5242485247497473`*^9, 3.5242499913234663`*^9, 
   3.5244001093188095`*^9, 3.5244002939729404`*^9, 3.5244062658109736`*^9, 
   3.524406546684946*^9, 3.52440678135618*^9, 3.5244078612372503`*^9, 
   3.5244154478237*^9, 3.5244155728548512`*^9, 3.5244158491636553`*^9, 
   3.5244159462702093`*^9, 3.524417909687304*^9, 3.5244180354974804`*^9, 
   3.524418224307745*^9, 3.524418268347807*^9, 3.524418390067977*^9, 
   3.5244186973299756`*^9, 3.524422220282406*^9, 3.5244223075543976`*^9, 
   3.524422748788635*^9, 3.5244238384219584`*^9, 3.5244422133339434`*^9, 
   3.5244427387569957`*^9, 3.524443845627305*^9, 3.5244456827323813`*^9, 
   3.5244458608565693`*^9, 3.5244468650735455`*^9, 3.5245024277533503`*^9, 
   3.524511940918826*^9, 3.5245883954350815`*^9, 3.525090283351362*^9, 
   3.5250916914404593`*^9, 3.525091948318575*^9, 3.525092186390678*^9, 
   3.5250924416737833`*^9, 3.5250925673782525`*^9, 3.5250926856510973`*^9, 
   3.525092889896757*^9, 3.5250932015706353`*^9, 3.5250987617346582`*^9, 
   3.5254479851077175`*^9, {3.525461187947706*^9, 3.5254612126121173`*^9}, 
   3.525518263574353*^9, 3.525824478456106*^9, 3.527177462454658*^9, 
   3.527178718086236*^9}],

Cell[BoxData[
 FormBox["0", TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5206959629626102`*^9, 3.520696070780498*^9, 3.520696278715569*^9, {
   3.520701513696095*^9, 3.5207015403271513`*^9}, 3.520702765231444*^9, 
   3.52070437802107*^9, 3.5207099901878777`*^9, 3.520710127740677*^9, 
   3.5207159882149105`*^9, 3.520716368042636*^9, 3.5207347252676086`*^9, 
   3.5207352293354397`*^9, 3.5207876785073557`*^9, 3.520788873138685*^9, 
   3.5208972346630206`*^9, 3.521303722618685*^9, 3.5213038422055244`*^9, 
   3.521486042482182*^9, 3.5214879118661046`*^9, 3.522245115133789*^9, 
   3.522245680701138*^9, 3.522246790380608*^9, 3.5222498953139777`*^9, 
   3.5223307063999023`*^9, 3.522356608146308*^9, 3.522356913152425*^9, 
   3.522429651370442*^9, 3.5225102547904625`*^9, 3.5225118663016353`*^9, 
   3.5227081342657146`*^9, 3.5227751527686853`*^9, 3.5227768397965193`*^9, 
   3.523028577736314*^9, 3.5230306325597553`*^9, 3.5230313316658297`*^9, 
   3.5235572651211634`*^9, 3.523724871912518*^9, 3.523726563631279*^9, 
   3.5239837419440365`*^9, 3.5239849894283886`*^9, 3.5239893042871847`*^9, 
   3.5239903280797424`*^9, 3.523992021605606*^9, 3.523992133239991*^9, 
   3.5239948150013795`*^9, 3.523994924738656*^9, 3.5239950356489997`*^9, 
   3.524248137665607*^9, 3.5242485247497473`*^9, 3.5242499913234663`*^9, 
   3.5244001093188095`*^9, 3.5244002939729404`*^9, 3.5244062658109736`*^9, 
   3.524406546684946*^9, 3.52440678135618*^9, 3.5244078612372503`*^9, 
   3.5244154478237*^9, 3.5244155728548512`*^9, 3.5244158491636553`*^9, 
   3.5244159462702093`*^9, 3.524417909687304*^9, 3.5244180354974804`*^9, 
   3.524418224307745*^9, 3.524418268347807*^9, 3.524418390067977*^9, 
   3.5244186973299756`*^9, 3.524422220282406*^9, 3.5244223075543976`*^9, 
   3.524422748788635*^9, 3.5244238384219584`*^9, 3.5244422133339434`*^9, 
   3.5244427387569957`*^9, 3.524443845627305*^9, 3.5244456827323813`*^9, 
   3.5244458608565693`*^9, 3.5244468650735455`*^9, 3.5245024277533503`*^9, 
   3.524511940918826*^9, 3.5245883954350815`*^9, 3.525090283351362*^9, 
   3.5250916914404593`*^9, 3.525091948318575*^9, 3.525092186390678*^9, 
   3.5250924416737833`*^9, 3.5250925673782525`*^9, 3.5250926856510973`*^9, 
   3.525092889896757*^9, 3.5250932015706353`*^9, 3.5250987617346582`*^9, 
   3.5254479851077175`*^9, {3.525461187947706*^9, 3.5254612126121173`*^9}, 
   3.525518263574353*^9, 3.525824478456106*^9, 3.527177462454658*^9, 
   3.5271787180895147`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"LinearStorso", "=", " ", 
  RowBox[{"pt1", "+", "pt2", "+", "pt3", "+", "pt4", "+", "pt5"}]}]], "Input",\

 CellChangeTimes->{
  3.5206959756855717`*^9, {3.520696261548789*^9, 3.520696261724345*^9}, {
   3.520709821738123*^9, 3.5207098238014174`*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   RowBox[{"-", 
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "(", "t", ")"}]}], "-", 
   FractionBox[
    RowBox[{"514", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], "1199"]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.520695975952197*^9, 3.520696070880694*^9, {3.5206962629949636`*^9, 
   3.5206962814600563`*^9}, {3.520701513854748*^9, 3.520701542041128*^9}, 
   3.5207027653535013`*^9, 3.520704378103973*^9, 3.520709990270878*^9, 
   3.520710127833252*^9, 3.520715988248913*^9, 3.520716368072637*^9, 
   3.520734725303611*^9, 3.5207352293684416`*^9, 3.520787678540358*^9, 
   3.520788873171687*^9, 3.520897234706023*^9, 3.521303722645686*^9, 
   3.5213038424025364`*^9, 3.5214860425231843`*^9, 3.5214879119021063`*^9, 
   3.522245115216794*^9, 3.5222456807531404`*^9, 3.522246790431611*^9, 
   3.5222498953639803`*^9, 3.522330706445905*^9, 3.5223566081775074`*^9, 
   3.5223569132148247`*^9, 3.5224296513854427`*^9, 3.522510254840465*^9, 
   3.52251186638264*^9, 3.522708134553731*^9, 3.5227751528016877`*^9, 
   3.5227768398375216`*^9, 3.5230285778113184`*^9, 3.5230306325787563`*^9, 
   3.5230313317028317`*^9, 3.5235572651501656`*^9, 3.5237248719455194`*^9, 
   3.5237265636482797`*^9, 3.523983741968038*^9, 3.52398498945039*^9, 
   3.5239893043071856`*^9, 3.523990328109744*^9, 3.5239920216306076`*^9, 
   3.523992133270993*^9, 3.523994815029381*^9, 3.5239949247646575`*^9, 
   3.523995035674001*^9, 3.5242481376916084`*^9, 3.524248524781749*^9, 
   3.524249991367469*^9, 3.5244001093288097`*^9, 3.5244002939889417`*^9, 
   3.5244062658299747`*^9, 3.5244065467149477`*^9, 3.524406781386182*^9, 
   3.524407861277252*^9, 3.524415447843701*^9, 3.5244155728738527`*^9, 
   3.524415849191657*^9, 3.52441594628521*^9, 3.524417909707304*^9, 
   3.52441803552748*^9, 3.524418224337745*^9, 3.5244182683778067`*^9, 
   3.5244183900879774`*^9, 3.524418697334976*^9, 3.524422220313408*^9, 
   3.524422307584399*^9, 3.5244227487946353`*^9, 3.5244238384269586`*^9, 
   3.524442213354944*^9, 3.5244427387809973`*^9, 3.524443845655307*^9, 
   3.5244456827653832`*^9, 3.524445860883571*^9, 3.524446865116548*^9, 
   3.524502427797353*^9, 3.5245119409478283`*^9, 3.5245883954520817`*^9, 
   3.5250902833783636`*^9, 3.525091691475461*^9, 3.5250919483585753`*^9, 
   3.52509218642968*^9, 3.525092441698785*^9, 3.525092567407254*^9, 
   3.5250926856951*^9, 3.5250928899447603`*^9, 3.525093201618638*^9, 
   3.5250987617666597`*^9, 3.5254479851317186`*^9, {3.525461187974708*^9, 
   3.5254612126391187`*^9}, 3.5255182636053553`*^9, 3.525824478486108*^9, 
   3.527177462508753*^9, 3.5271787181764803`*^9}]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Non-stance Slope", "Subsection",
 CellChangeTimes->{{3.520696369485465*^9, 3.520696373189542*^9}, {
  3.52717746597353*^9, 3.52717746813941*^9}}],

Cell[CellGroupData[{

Cell["non-stance slope", "Subsubsection",
 CellChangeTimes->{{3.520696408621931*^9, 3.5206964120770273`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"nsslope", " ", "=", " ", 
  RowBox[{
   FractionBox[
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
     RowBox[{
      RowBox[{
       SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
     RowBox[{
      RowBox[{
       SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", 
   "p0"}]}]], "Input",
 CellChangeTimes->{{3.520696380896888*^9, 3.5206963990299892`*^9}, {
  3.520696437432332*^9, 3.520696439262645*^9}, {3.520709844151616*^9, 
  3.520709845340612*^9}}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"685", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
       ")"}]}], "250000"], "+", 
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"-", 
         RowBox[{"685", " ", 
          RowBox[{"sin", "(", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "-", 
        RowBox[{"514", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"685", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
       ")"}]}], "250000"]}], 
   RowBox[{
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"685", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"514", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"685", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
       ")"}]}], "250000"], "-", 
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"685", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
       ")"}]}], "250000"]}]], TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5206963995801153`*^9, 3.520696439845653*^9, 3.520701546635215*^9, 
   3.520702770657765*^9, 3.520704382692731*^9, 3.520709990346691*^9, 
   3.5207101364846*^9, 3.520715988318917*^9, 3.520716368140641*^9, 
   3.520734725380615*^9, 3.5207352294344454`*^9, 3.520787678612362*^9, 
   3.520788873238691*^9, 3.5208972347830276`*^9, 3.5213037227466927`*^9, 
   3.5213038425315437`*^9, 3.521486042709195*^9, 3.52148791196811*^9, 
   3.5222451152847977`*^9, 3.5222456808321457`*^9, 3.5222467905106153`*^9, 
   3.5222498954479856`*^9, 3.5223307065239096`*^9, 3.5223566082399073`*^9, 
   3.522356913277225*^9, 3.522429651445446*^9, 3.5225102549354706`*^9, 
   3.522511866499647*^9, 3.522708134638736*^9, 3.522775152825689*^9, 
   3.522776839921526*^9, 3.5230285779333253`*^9, 3.523030632651761*^9, 
   3.5230313318168383`*^9, 3.5235572652171693`*^9, 3.5237248719785213`*^9, 
   3.523726563664281*^9, 3.5239837419910393`*^9, 3.523984989467391*^9, 
   3.5239893043221865`*^9, 3.5239903281387453`*^9, 3.523992021645609*^9, 
   3.523992133285994*^9, 3.5239948150523825`*^9, 3.5239949247896585`*^9, 
   3.523995035694002*^9, 3.52424813771461*^9, 3.5242485248107505`*^9, 
   3.5242499914574738`*^9, 3.5244001093488097`*^9, 3.5244002940019426`*^9, 
   3.524406265852976*^9, 3.5244065467449493`*^9, 3.5244067814141836`*^9, 
   3.5244078613132544`*^9, 3.524415447861702*^9, 3.524415572888853*^9, 
   3.52441584924866*^9, 3.524415946295211*^9, 3.5244179097373037`*^9, 
   3.52441803556748*^9, 3.524418224357745*^9, 3.524418268397807*^9, 
   3.524418390107977*^9, 3.5244186973409767`*^9, 3.5244222203474097`*^9, 
   3.5244223076154013`*^9, 3.5244227488006353`*^9, 3.524423838429959*^9, 
   3.5244422133729453`*^9, 3.5244427388079987`*^9, 3.5244438456863084`*^9, 
   3.524445682797385*^9, 3.5244458609055724`*^9, 3.5244468651925526`*^9, 
   3.524502427891358*^9, 3.524511941020832*^9, 3.524588395474083*^9, 
   3.5250902834133654`*^9, 3.525091691513463*^9, 3.5250919483885756`*^9, 
   3.5250921864616823`*^9, 3.5250924417217865`*^9, 3.5250925674312553`*^9, 
   3.5250926857341022`*^9, 3.525092889964761*^9, 3.5250932016666403`*^9, 
   3.525098761796662*^9, 3.5254479851827216`*^9, {3.52546118800371*^9, 
   3.52546121266212*^9}, 3.525518263635357*^9, 3.525824478507109*^9, 
   3.527177472451507*^9, 3.527178718369808*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["linearized non-stance slope", "Subsubsection",
 CellChangeTimes->{{3.520696416053858*^9, 3.5206964219347477`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"LinearNSslopePre", "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{"Normal", "[", 
    RowBox[{"Series", "[", 
     RowBox[{
      RowBox[{
       FractionBox[
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", "p0"}],
       ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], ",", "0", ",", "1"}],
        "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], ",", "0", ",", "1"}],
        "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], ",", "0", ",", "1"}],
        "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], ",", "0", ",", "1"}],
        "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], ",", "0", ",", "1"}],
        "}"}]}], "]"}], "]"}], "]"}]}]], "Input",
 CellChangeTimes->{{3.520696435856447*^9, 3.5206964481735373`*^9}, 
   3.520701453361051*^9, {3.52070709267367*^9, 3.5207070971639357`*^9}, {
   3.520709837143972*^9, 3.5207098403460407`*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   RowBox[{"-", 
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "(", "t", ")"}]}], "-", 
   RowBox[{
    SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "-", 
   RowBox[{
    RowBox[{"(", 
     RowBox[{
      RowBox[{"2", " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], "+", "1"}], ")"}], 
    " ", 
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], "+", 
   RowBox[{
    RowBox[{"(", 
     RowBox[{
      RowBox[{"2", " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], "+", 
      RowBox[{"2", " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
      "1"}], ")"}], " ", 
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], "-", 
   FractionBox[
    RowBox[{"685", " ", 
     RowBox[{"(", 
      RowBox[{
       RowBox[{"-", 
        RowBox[{"2", " ", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], "+", 
       
       RowBox[{"2", " ", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], " ", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], "+", 
       RowBox[{"2", " ", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"-", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "+", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"8", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], "-", "1"}], 
            ")"}]}]}], ")"}]}], "-", "1"}], ")"}], " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], "1199"]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{{3.520696442764336*^9, 3.520696448753223*^9}, 
   3.5207014542994947`*^9, 3.520701548789372*^9, 3.520702771198017*^9, 
   3.5207043831593*^9, 3.520707098089073*^9, 3.520709990838419*^9, 
   3.520710137038608*^9, 3.5207159884199224`*^9, 3.520716368243647*^9, 
   3.5207347254886217`*^9, 3.520735229533451*^9, 3.520787678712368*^9, 
   3.520788873348697*^9, 3.5208972348910336`*^9, 3.521303723076711*^9, 
   3.52130384282056*^9, 3.5214860428572035`*^9, 3.5214879121211195`*^9, 
   3.522245115591816*^9, 3.5222456809431515`*^9, 3.522246790625622*^9, 
   3.522249895569992*^9, 3.5223307066409163`*^9, 3.5223566083491077`*^9, 
   3.522356913386425*^9, 3.5224296515344515`*^9, 3.522510255068478*^9, 
   3.522511866679657*^9, 3.5227081347927446`*^9, 3.522775152891693*^9, 
   3.5227768400305324`*^9, 3.5230285780793333`*^9, 3.5230306327627673`*^9, 
   3.5230313319288445`*^9, 3.5235572653161745`*^9, 3.5237248720475254`*^9, 
   3.5237265637172832`*^9, 3.523983742048043*^9, 3.523984989518394*^9, 
   3.5239893043721895`*^9, 3.523990328201749*^9, 3.5239920216986113`*^9, 
   3.5239921333399973`*^9, 3.5239948151053853`*^9, 3.5239949248496623`*^9, 
   3.5239950357470055`*^9, 3.5242481377726135`*^9, 3.524248524872754*^9, 
   3.524249991577481*^9, 3.52440010939881*^9, 3.5244002940509453`*^9, 
   3.5244062659109793`*^9, 3.5244065468099537`*^9, 3.524406781478187*^9, 
   3.5244078613852587`*^9, 3.5244154479097047`*^9, 3.5244155729378557`*^9, 
   3.5244158492976627`*^9, 3.5244159463402133`*^9, 3.524417909797304*^9, 
   3.5244180356274805`*^9, 3.524418224417745*^9, 3.524418268407807*^9, 
   3.5244183901679773`*^9, 3.5244186973819785`*^9, 3.5244222204144135`*^9, 
   3.524422307682405*^9, 3.524422748841638*^9, 3.5244238384679613`*^9, 
   3.5244422134249487`*^9, 3.5244427388640018`*^9, 3.5244438457513123`*^9, 
   3.524445682865389*^9, 3.524445860964576*^9, 3.5244468652995586`*^9, 
   3.5245024280033646`*^9, 3.524511941135839*^9, 3.524588395533087*^9, 
   3.525090283480369*^9, 3.5250916915864677`*^9, 3.5250919484585752`*^9, 
   3.5250921865336857`*^9, 3.5250924417837896`*^9, 3.525092567497259*^9, 
   3.525092685816107*^9, 3.525092890032765*^9, 3.5250932017666464`*^9, 
   3.5250987618786664`*^9, 3.5254479852707267`*^9, {3.525461188068713*^9, 
   3.5254612127231236`*^9}, 3.5255182637223616`*^9, 3.525824478581113*^9, 
   3.52717747267846*^9, 3.527178718518577*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{"pt1", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearNSslopePre", ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"pt2", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearNSslopePre", ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"pt3", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearNSslopePre", ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"pt4", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearNSslopePre", ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"pt5", " ", "=", " ", 
  RowBox[{
   RowBox[{"Collect", "[", 
    RowBox[{"LinearNSslopePre", ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", "0"}], 
     ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
      "0"}]}], "}"}]}]}]}], "Input",
 CellChangeTimes->{{3.520696462109189*^9, 3.520696489889127*^9}, {
  3.520709853827671*^9, 3.520709865118373*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"-", 
   RowBox[{
    SubscriptBox["\[Theta]", "1"], "(", "t", ")"}]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5206964908441963`*^9, 3.520701551075437*^9, 3.520702771268022*^9, 
   3.52070438319821*^9, 3.5207099909985447`*^9, 3.520710137101225*^9, 
   3.520715988446924*^9, 3.5207163682676487`*^9, 3.520734725522623*^9, 
   3.520735229561453*^9, 3.5207876787443695`*^9, 3.520788873373698*^9, 
   3.5208972349270353`*^9, 3.5213037231257143`*^9, 3.5213038428835635`*^9, 
   3.521486042901206*^9, 3.521487912154121*^9, 3.5222451156288176`*^9, 
   3.522245680992154*^9, 3.5222467906726246`*^9, 3.522249895615995*^9, 
   3.5223307066769185`*^9, 3.522356608380308*^9, 3.522356913433225*^9, 
   3.5224296515774536`*^9, 3.522510255119481*^9, 3.522511866744661*^9, 
   3.522708134866749*^9, 3.5227751529166937`*^9, 3.522776840074535*^9, 
   3.5230285781483374`*^9, 3.523030632778768*^9, 3.523031332129856*^9, 
   3.52355726540718*^9, 3.5237248720825276`*^9, 3.5237265637322845`*^9, 
   3.523983742072044*^9, 3.5239849895353947`*^9, 3.5239893043871903`*^9, 
   3.523990328230751*^9, 3.523992021721613*^9, 3.523992133355998*^9, 
   3.5239948151243863`*^9, 3.5239949248756638`*^9, 3.5239950357660065`*^9, 
   3.5242481377986145`*^9, 3.5242485249027557`*^9, 3.524249991619483*^9, 
   3.52440010941881*^9, 3.5244002940699463`*^9, 3.524406265935981*^9, 
   3.5244065468399553`*^9, 3.524406781505189*^9, 3.5244078614222603`*^9, 
   3.524415447925706*^9, 3.5244155729538565`*^9, 3.524415849314664*^9, 
   3.524415946351214*^9, 3.524417909827304*^9, 3.5244180356574802`*^9, 
   3.524418224437745*^9, 3.524418268427807*^9, 3.524418390197977*^9, 
   3.5244186973879786`*^9, 3.5244222204464154`*^9, 3.524422307715407*^9, 
   3.5244227488486385`*^9, 3.524423838470961*^9, 3.5244422134469495`*^9, 
   3.524442738886003*^9, 3.524443845785314*^9, 3.524445682898391*^9, 
   3.524445860986577*^9, 3.524446865335561*^9, 3.5245024280483665`*^9, 
   3.524511941168841*^9, 3.524588395554088*^9, 3.5250902835113707`*^9, 
   3.5250916916224694`*^9, 3.525091948488576*^9, 3.52509218660369*^9, 
   3.5250924418047905`*^9, 3.525092567525261*^9, 3.5250926858551087`*^9, 
   3.5250928900597663`*^9, 3.5250932018186493`*^9, 3.525098761902668*^9, 
   3.525447985298728*^9, {3.525461190066828*^9, 3.5254612127471247`*^9}, 
   3.5255182637533636`*^9, 3.525824478607115*^9, 3.527177472772882*^9, 
   3.527178718588883*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"-", 
   RowBox[{
    SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5206964908441963`*^9, 3.520701551075437*^9, 3.520702771268022*^9, 
   3.52070438319821*^9, 3.5207099909985447`*^9, 3.520710137101225*^9, 
   3.520715988446924*^9, 3.5207163682676487`*^9, 3.520734725522623*^9, 
   3.520735229561453*^9, 3.5207876787443695`*^9, 3.520788873373698*^9, 
   3.5208972349270353`*^9, 3.5213037231257143`*^9, 3.5213038428835635`*^9, 
   3.521486042901206*^9, 3.521487912154121*^9, 3.5222451156288176`*^9, 
   3.522245680992154*^9, 3.5222467906726246`*^9, 3.522249895615995*^9, 
   3.5223307066769185`*^9, 3.522356608380308*^9, 3.522356913433225*^9, 
   3.5224296515774536`*^9, 3.522510255119481*^9, 3.522511866744661*^9, 
   3.522708134866749*^9, 3.5227751529166937`*^9, 3.522776840074535*^9, 
   3.5230285781483374`*^9, 3.523030632778768*^9, 3.523031332129856*^9, 
   3.52355726540718*^9, 3.5237248720825276`*^9, 3.5237265637322845`*^9, 
   3.523983742072044*^9, 3.5239849895353947`*^9, 3.5239893043871903`*^9, 
   3.523990328230751*^9, 3.523992021721613*^9, 3.523992133355998*^9, 
   3.5239948151243863`*^9, 3.5239949248756638`*^9, 3.5239950357660065`*^9, 
   3.5242481377986145`*^9, 3.5242485249027557`*^9, 3.524249991619483*^9, 
   3.52440010941881*^9, 3.5244002940699463`*^9, 3.524406265935981*^9, 
   3.5244065468399553`*^9, 3.524406781505189*^9, 3.5244078614222603`*^9, 
   3.524415447925706*^9, 3.5244155729538565`*^9, 3.524415849314664*^9, 
   3.524415946351214*^9, 3.524417909827304*^9, 3.5244180356574802`*^9, 
   3.524418224437745*^9, 3.524418268427807*^9, 3.524418390197977*^9, 
   3.5244186973879786`*^9, 3.5244222204464154`*^9, 3.524422307715407*^9, 
   3.5244227488486385`*^9, 3.524423838470961*^9, 3.5244422134469495`*^9, 
   3.524442738886003*^9, 3.524443845785314*^9, 3.524445682898391*^9, 
   3.524445860986577*^9, 3.524446865335561*^9, 3.5245024280483665`*^9, 
   3.524511941168841*^9, 3.524588395554088*^9, 3.5250902835113707`*^9, 
   3.5250916916224694`*^9, 3.525091948488576*^9, 3.52509218660369*^9, 
   3.5250924418047905`*^9, 3.525092567525261*^9, 3.5250926858551087`*^9, 
   3.5250928900597663`*^9, 3.5250932018186493`*^9, 3.525098761902668*^9, 
   3.525447985298728*^9, {3.525461190066828*^9, 3.5254612127471247`*^9}, 
   3.5255182637533636`*^9, 3.525824478607115*^9, 3.527177472772882*^9, 
   3.527178718590207*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"-", 
   RowBox[{
    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5206964908441963`*^9, 3.520701551075437*^9, 3.520702771268022*^9, 
   3.52070438319821*^9, 3.5207099909985447`*^9, 3.520710137101225*^9, 
   3.520715988446924*^9, 3.5207163682676487`*^9, 3.520734725522623*^9, 
   3.520735229561453*^9, 3.5207876787443695`*^9, 3.520788873373698*^9, 
   3.5208972349270353`*^9, 3.5213037231257143`*^9, 3.5213038428835635`*^9, 
   3.521486042901206*^9, 3.521487912154121*^9, 3.5222451156288176`*^9, 
   3.522245680992154*^9, 3.5222467906726246`*^9, 3.522249895615995*^9, 
   3.5223307066769185`*^9, 3.522356608380308*^9, 3.522356913433225*^9, 
   3.5224296515774536`*^9, 3.522510255119481*^9, 3.522511866744661*^9, 
   3.522708134866749*^9, 3.5227751529166937`*^9, 3.522776840074535*^9, 
   3.5230285781483374`*^9, 3.523030632778768*^9, 3.523031332129856*^9, 
   3.52355726540718*^9, 3.5237248720825276`*^9, 3.5237265637322845`*^9, 
   3.523983742072044*^9, 3.5239849895353947`*^9, 3.5239893043871903`*^9, 
   3.523990328230751*^9, 3.523992021721613*^9, 3.523992133355998*^9, 
   3.5239948151243863`*^9, 3.5239949248756638`*^9, 3.5239950357660065`*^9, 
   3.5242481377986145`*^9, 3.5242485249027557`*^9, 3.524249991619483*^9, 
   3.52440010941881*^9, 3.5244002940699463`*^9, 3.524406265935981*^9, 
   3.5244065468399553`*^9, 3.524406781505189*^9, 3.5244078614222603`*^9, 
   3.524415447925706*^9, 3.5244155729538565`*^9, 3.524415849314664*^9, 
   3.524415946351214*^9, 3.524417909827304*^9, 3.5244180356574802`*^9, 
   3.524418224437745*^9, 3.524418268427807*^9, 3.524418390197977*^9, 
   3.5244186973879786`*^9, 3.5244222204464154`*^9, 3.524422307715407*^9, 
   3.5244227488486385`*^9, 3.524423838470961*^9, 3.5244422134469495`*^9, 
   3.524442738886003*^9, 3.524443845785314*^9, 3.524445682898391*^9, 
   3.524445860986577*^9, 3.524446865335561*^9, 3.5245024280483665`*^9, 
   3.524511941168841*^9, 3.524588395554088*^9, 3.5250902835113707`*^9, 
   3.5250916916224694`*^9, 3.525091948488576*^9, 3.52509218660369*^9, 
   3.5250924418047905`*^9, 3.525092567525261*^9, 3.5250926858551087`*^9, 
   3.5250928900597663`*^9, 3.5250932018186493`*^9, 3.525098761902668*^9, 
   3.525447985298728*^9, {3.525461190066828*^9, 3.5254612127471247`*^9}, 
   3.5255182637533636`*^9, 3.525824478607115*^9, 3.527177472772882*^9, 
   3.527178718591269*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{
   SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], TraditionalForm]], "Output",\

 CellChangeTimes->{
  3.5206964908441963`*^9, 3.520701551075437*^9, 3.520702771268022*^9, 
   3.52070438319821*^9, 3.5207099909985447`*^9, 3.520710137101225*^9, 
   3.520715988446924*^9, 3.5207163682676487`*^9, 3.520734725522623*^9, 
   3.520735229561453*^9, 3.5207876787443695`*^9, 3.520788873373698*^9, 
   3.5208972349270353`*^9, 3.5213037231257143`*^9, 3.5213038428835635`*^9, 
   3.521486042901206*^9, 3.521487912154121*^9, 3.5222451156288176`*^9, 
   3.522245680992154*^9, 3.5222467906726246`*^9, 3.522249895615995*^9, 
   3.5223307066769185`*^9, 3.522356608380308*^9, 3.522356913433225*^9, 
   3.5224296515774536`*^9, 3.522510255119481*^9, 3.522511866744661*^9, 
   3.522708134866749*^9, 3.5227751529166937`*^9, 3.522776840074535*^9, 
   3.5230285781483374`*^9, 3.523030632778768*^9, 3.523031332129856*^9, 
   3.52355726540718*^9, 3.5237248720825276`*^9, 3.5237265637322845`*^9, 
   3.523983742072044*^9, 3.5239849895353947`*^9, 3.5239893043871903`*^9, 
   3.523990328230751*^9, 3.523992021721613*^9, 3.523992133355998*^9, 
   3.5239948151243863`*^9, 3.5239949248756638`*^9, 3.5239950357660065`*^9, 
   3.5242481377986145`*^9, 3.5242485249027557`*^9, 3.524249991619483*^9, 
   3.52440010941881*^9, 3.5244002940699463`*^9, 3.524406265935981*^9, 
   3.5244065468399553`*^9, 3.524406781505189*^9, 3.5244078614222603`*^9, 
   3.524415447925706*^9, 3.5244155729538565`*^9, 3.524415849314664*^9, 
   3.524415946351214*^9, 3.524417909827304*^9, 3.5244180356574802`*^9, 
   3.524418224437745*^9, 3.524418268427807*^9, 3.524418390197977*^9, 
   3.5244186973879786`*^9, 3.5244222204464154`*^9, 3.524422307715407*^9, 
   3.5244227488486385`*^9, 3.524423838470961*^9, 3.5244422134469495`*^9, 
   3.524442738886003*^9, 3.524443845785314*^9, 3.524445682898391*^9, 
   3.524445860986577*^9, 3.524446865335561*^9, 3.5245024280483665`*^9, 
   3.524511941168841*^9, 3.524588395554088*^9, 3.5250902835113707`*^9, 
   3.5250916916224694`*^9, 3.525091948488576*^9, 3.52509218660369*^9, 
   3.5250924418047905`*^9, 3.525092567525261*^9, 3.5250926858551087`*^9, 
   3.5250928900597663`*^9, 3.5250932018186493`*^9, 3.525098761902668*^9, 
   3.525447985298728*^9, {3.525461190066828*^9, 3.5254612127471247`*^9}, 
   3.5255182637533636`*^9, 3.525824478607115*^9, 3.527177472772882*^9, 
   3.527178718592366*^9}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{"685", " ", 
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], "1199"], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5206964908441963`*^9, 3.520701551075437*^9, 3.520702771268022*^9, 
   3.52070438319821*^9, 3.5207099909985447`*^9, 3.520710137101225*^9, 
   3.520715988446924*^9, 3.5207163682676487`*^9, 3.520734725522623*^9, 
   3.520735229561453*^9, 3.5207876787443695`*^9, 3.520788873373698*^9, 
   3.5208972349270353`*^9, 3.5213037231257143`*^9, 3.5213038428835635`*^9, 
   3.521486042901206*^9, 3.521487912154121*^9, 3.5222451156288176`*^9, 
   3.522245680992154*^9, 3.5222467906726246`*^9, 3.522249895615995*^9, 
   3.5223307066769185`*^9, 3.522356608380308*^9, 3.522356913433225*^9, 
   3.5224296515774536`*^9, 3.522510255119481*^9, 3.522511866744661*^9, 
   3.522708134866749*^9, 3.5227751529166937`*^9, 3.522776840074535*^9, 
   3.5230285781483374`*^9, 3.523030632778768*^9, 3.523031332129856*^9, 
   3.52355726540718*^9, 3.5237248720825276`*^9, 3.5237265637322845`*^9, 
   3.523983742072044*^9, 3.5239849895353947`*^9, 3.5239893043871903`*^9, 
   3.523990328230751*^9, 3.523992021721613*^9, 3.523992133355998*^9, 
   3.5239948151243863`*^9, 3.5239949248756638`*^9, 3.5239950357660065`*^9, 
   3.5242481377986145`*^9, 3.5242485249027557`*^9, 3.524249991619483*^9, 
   3.52440010941881*^9, 3.5244002940699463`*^9, 3.524406265935981*^9, 
   3.5244065468399553`*^9, 3.524406781505189*^9, 3.5244078614222603`*^9, 
   3.524415447925706*^9, 3.5244155729538565`*^9, 3.524415849314664*^9, 
   3.524415946351214*^9, 3.524417909827304*^9, 3.5244180356574802`*^9, 
   3.524418224437745*^9, 3.524418268427807*^9, 3.524418390197977*^9, 
   3.5244186973879786`*^9, 3.5244222204464154`*^9, 3.524422307715407*^9, 
   3.5244227488486385`*^9, 3.524423838470961*^9, 3.5244422134469495`*^9, 
   3.524442738886003*^9, 3.524443845785314*^9, 3.524445682898391*^9, 
   3.524445860986577*^9, 3.524446865335561*^9, 3.5245024280483665`*^9, 
   3.524511941168841*^9, 3.524588395554088*^9, 3.5250902835113707`*^9, 
   3.5250916916224694`*^9, 3.525091948488576*^9, 3.52509218660369*^9, 
   3.5250924418047905`*^9, 3.525092567525261*^9, 3.5250926858551087`*^9, 
   3.5250928900597663`*^9, 3.5250932018186493`*^9, 3.525098761902668*^9, 
   3.525447985298728*^9, {3.525461190066828*^9, 3.5254612127471247`*^9}, 
   3.5255182637533636`*^9, 3.525824478607115*^9, 3.527177472772882*^9, 
   3.5271787185936823`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"LinearNSslope", "=", " ", 
  RowBox[{"pt1", "+", "pt2", "+", "pt3", "+", "pt4", "+", "pt5"}]}]], "Input",\

 CellChangeTimes->{{3.5206965003536177`*^9, 3.5206965017517843`*^9}, {
  3.5207098713951817`*^9, 3.520709872730335*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   RowBox[{"-", 
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "(", "t", ")"}]}], "-", 
   RowBox[{
    SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "-", 
   RowBox[{
    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "+", 
   RowBox[{
    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "+", 
   FractionBox[
    RowBox[{"685", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], "1199"]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.520696502255287*^9, 3.520701552567511*^9, 3.520702771346013*^9, 
   3.520704383271482*^9, 3.5207099911117773`*^9, 3.5207101371838827`*^9, 
   3.520715988480926*^9, 3.52071636829965*^9, 3.520734725562626*^9, 
   3.5207352295914545`*^9, 3.520787678778372*^9, 3.5207888734237013`*^9, 
   3.5208972349720383`*^9, 3.5213037231727166`*^9, 3.521303842923566*^9, 
   3.521486042948209*^9, 3.5214879121881227`*^9, 3.5222451156658196`*^9, 
   3.522245681046158*^9, 3.5222467907236276`*^9, 3.5222498956659975`*^9, 
   3.5223307067299213`*^9, 3.522356608411508*^9, 3.5223569134800253`*^9, 
   3.5224296515874543`*^9, 3.522510255162483*^9, 3.5225118668276653`*^9, 
   3.522708135032758*^9, 3.522775152946696*^9, 3.5227768401175375`*^9, 
   3.5230285782193413`*^9, 3.5230306327997694`*^9, 3.5230313321638584`*^9, 
   3.523557265443182*^9, 3.52372487211753*^9, 3.523726563752286*^9, 
   3.5239837420960455`*^9, 3.5239849895583963`*^9, 3.5239893044071913`*^9, 
   3.5239903282627525`*^9, 3.5239920217466145`*^9, 3.5239921333789997`*^9, 
   3.523994815155388*^9, 3.523994924902665*^9, 3.523995035796008*^9, 
   3.5242481378276167`*^9, 3.524248524933758*^9, 3.5242499916614857`*^9, 
   3.5244001094388103`*^9, 3.5244002940909476`*^9, 3.5244062659609823`*^9, 
   3.5244065468759575`*^9, 3.5244067815361905`*^9, 3.524407861464263*^9, 
   3.5244154479397063`*^9, 3.524415572972858*^9, 3.524415849334665*^9, 
   3.524415946365215*^9, 3.5244179098573046`*^9, 3.5244180356874804`*^9, 
   3.5244182244677453`*^9, 3.524418268447807*^9, 3.5244183902179775`*^9, 
   3.524418697392979*^9, 3.524422220479417*^9, 3.524422307748409*^9, 
   3.5244227488536386`*^9, 3.5244238384759617`*^9, 3.5244422134689507`*^9, 
   3.5244427389120045`*^9, 3.5244438458163157`*^9, 3.5244456829313927`*^9, 
   3.5244458610125785`*^9, 3.524446865373563*^9, 3.52450242809437*^9, 
   3.5245119412048426`*^9, 3.524588395576089*^9, 3.5250902835423727`*^9, 
   3.5250916916594715`*^9, 3.5250919485185757`*^9, 3.5250921866306915`*^9, 
   3.5250924418297925`*^9, 3.5250925675532627`*^9, 3.5250926858981113`*^9, 
   3.5250928900897684`*^9, 3.5250932018686523`*^9, 3.52509876194567*^9, 
   3.52544798532473*^9, {3.5254611901328316`*^9, 3.5254612127731266`*^9}, 
   3.525518263782365*^9, 3.5258244786351166`*^9, 3.5271774728446903`*^9, 
   3.527178718643497*^9}]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Hip Angle", "Subsection",
 CellChangeTimes->{{3.520696541620419*^9, 3.52069656883606*^9}}],

Cell[CellGroupData[{

Cell["Hip Angle", "Subsubsection",
 CellChangeTimes->{{3.520696574436529*^9, 3.52069657648393*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"HipAngle", " ", "=", " ", 
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
   RowBox[{
    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}]}]], "Input",
 CellChangeTimes->{{3.52069657822197*^9, 3.5206965953274107`*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
   RowBox[{
    SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5206965960081053`*^9, 3.520702777353779*^9, 3.5207043853443203`*^9, 
   3.520709991187462*^9, 3.5207101403612432`*^9, 3.5207159885069275`*^9, 
   3.5207163683246517`*^9, 3.5207347255926275`*^9, 3.520735229613456*^9, 
   3.520787678805373*^9, 3.520788873447703*^9, 3.52089723500504*^9, 
   3.521303723245721*^9, 3.5213038429625683`*^9, 3.521486042986211*^9, 
   3.5214879122141247`*^9, 3.5222451156978216`*^9, 3.5222456810931606`*^9, 
   3.5222467907696304`*^9, 3.5222498957070003`*^9, 3.5223307067659235`*^9, 
   3.522356608427108*^9, 3.5223569135112257`*^9, 3.522429651594455*^9, 
   3.5225102552034855`*^9, 3.522511866895669*^9, 3.5227081351907673`*^9, 
   3.5227751529686975`*^9, 3.52277684015654*^9, 3.523028578280345*^9, 
   3.52303063281077*^9, 3.523031332231862*^9, 3.5235572654681835`*^9, 
   3.5237248721485314`*^9, 3.5237265637642865`*^9, 3.5239837421500483`*^9, 
   3.523984989576397*^9, 3.523989304421192*^9, 3.5239903282907543`*^9, 
   3.523992021764615*^9, 3.5239921333980007`*^9, 3.5239948151773896`*^9, 
   3.5239949249266667`*^9, 3.523995035812009*^9, 3.5242481378486176`*^9, 
   3.524248524962759*^9, 3.5242499916984878`*^9, 3.52440010944881*^9, 
   3.524400294101948*^9, 3.524406265985984*^9, 3.5244065469069586`*^9, 
   3.524406781565192*^9, 3.5244078614992647`*^9, 3.524415447951707*^9, 
   3.524415572987859*^9, 3.5244158493496656`*^9, 3.5244159463752155`*^9, 
   3.524417909877304*^9, 3.5244180357174807`*^9, 3.5244182244877453`*^9, 
   3.524418268467807*^9, 3.5244183902379775`*^9, 3.5244186973949795`*^9, 
   3.5244222205074186`*^9, 3.5244223077784104`*^9, 3.524422748857639*^9, 
   3.5244238384769616`*^9, 3.524442213484952*^9, 3.5244427389340057`*^9, 
   3.524443845845318*^9, 3.5244456829643946`*^9, 3.52444586103358*^9, 
   3.5244468654045644`*^9, 3.5245024281313715`*^9, 3.5245119412358446`*^9, 
   3.52458839559309*^9, 3.5250902835683746`*^9, 3.5250916916914735`*^9, 
   3.5250919485485754`*^9, 3.525092186648693*^9, 3.5250924418527937`*^9, 
   3.5250925675762634`*^9, 3.525092685937114*^9, 3.5250928901137695`*^9, 
   3.525093201912655*^9, 3.525098761968672*^9, 3.525447985347731*^9, {
   3.5254611901498327`*^9, 3.5254612127971277`*^9}, 3.525518263807367*^9, 
   3.525824478653117*^9, 3.527178718737479*^9}]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Hip Position", "Subsection",
 CellChangeTimes->{{3.5206971864248543`*^9, 3.520697188943389*^9}}],

Cell[CellGroupData[{

Cell["Hip Position", "Subsubsection",
 CellChangeTimes->{{3.520697193656226*^9, 3.520697196463649*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"HipPos", " ", "=", " ", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
     "\[LeftDoubleBracket]", 
     RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "p0"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.5206971983837137`*^9, 3.5206972146818438`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["Linearized Hip Position", "Subsubsection",
 CellChangeTimes->{{3.52069721892881*^9, 3.520697223591626*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"LHipPos", " ", "=", " ", 
   RowBox[{
    RowBox[{
     RowBox[{"-", "Lc"}], " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
    RowBox[{"Lt", 
     RowBox[{"(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], ")"}]}]}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.52069723201585*^9, 3.520697274076376*^9}, 
   3.520707794253764*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["Dimensionless Linearized Hip Position", "Subsubsection",
 CellChangeTimes->{{3.520697279031687*^9, 3.520697286924871*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"LHipPosD", " ", "=", " ", 
   RowBox[{
    RowBox[{"-", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
    RowBox[{"(", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], ")"}]}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.5206973046773453`*^9, 3.5206973074500313`*^9}, {
  3.520707797700944*^9, 3.520707799451249*^9}}],

Cell[BoxData[""], "Input",
 CellChangeTimes->{3.527176876672881*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["COM", "Subsection",
 CellChangeTimes->{{3.527159931535033*^9, 3.527159931819805*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["g", "4"], "[", "\[Theta]", "]"}], "\[LeftDoubleBracket]", 
  RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]], "Input",
 CellChangeTimes->{{3.5271786073874493`*^9, 3.527178626780258*^9}}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{
    RowBox[{"500000", " ", 
     RowBox[{
      SubscriptBox["p", "x"], "(", "t", ")"}]}], "-", 
    RowBox[{"173990", " ", 
     RowBox[{"sin", "(", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
    RowBox[{"130556", " ", 
     RowBox[{"sin", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
    RowBox[{"66481", " ", 
     RowBox[{"sin", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
    RowBox[{"10", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], 
   "500000"], TraditionalForm]], "Output",
 CellChangeTimes->{{3.5271786039193563`*^9, 3.527178627309042*^9}, 
   3.527178718831472*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["p", "COM"], "=", 
  RowBox[{
   RowBox[{
    RowBox[{"(", 
     RowBox[{
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], 
        RowBox[{"Length", "[", "mm", "]"}]], 
       RowBox[{
        RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
        
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"2", "*", 
            RowBox[{"(", 
             RowBox[{"mc", "+", "mt"}], ")"}]}], "+", "mh"}], ")"}]}]}]}], "/.",
       "constsubs"}], ")"}], "//.", "p0"}], "//", "Simplify"}]}]], "Input",
 CellChangeTimes->{{3.515859389894578*^9, 3.515859428264773*^9}, {
   3.515859465154883*^9, 3.5158594906983433`*^9}, {3.515859551458819*^9, 
   3.515859552570883*^9}, {3.515859601883703*^9, 3.5158596025067387`*^9}, {
   3.515860689352903*^9, 3.5158607032496977`*^9}, 3.5158888908592005`*^9, {
   3.5158889225272565`*^9, 3.515888940763688*^9}, {3.515889859873678*^9, 
   3.5158898662696896`*^9}, {3.5159487112085867`*^9, 3.515948711738988*^9}, {
   3.5159488047463512`*^9, 3.5159488071019554`*^9}, {3.5159532190899763`*^9, 
   3.5159532199880276`*^9}, {3.515953578551536*^9, 3.51595358036364*^9}, {
   3.5159537466121483`*^9, 3.515953749921338*^9}, {3.5159854643716235`*^9, 
   3.515985465601694*^9}, {3.5271601133861094`*^9, 3.527160124146968*^9}, {
   3.527160247020347*^9, 3.5271603060203867`*^9}, {3.527160699015764*^9, 
   3.527160699824835*^9}, {3.527161199554492*^9, 3.527161205106392*^9}, {
   3.5271783365251904`*^9, 3.527178337066943*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   FractionBox["1", "122235500000"], 
   RowBox[{"(", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"41834171195", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "-", 
     RowBox[{"25096253337", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
     RowBox[{"401207755", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
     RowBox[{"6820902539", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
     RowBox[{"701338095", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
     RowBox[{"606150", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
     RowBox[{"9657960", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
     RowBox[{"606150", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], ")"}]}],
   TraditionalForm]], "Output",
 CellChangeTimes->{
  3.527160129283697*^9, 3.527160160599412*^9, {3.527160275629862*^9, 
   3.527160306604056*^9}, 3.527160700578635*^9, 3.527160733047275*^9, 
   3.527161207074335*^9, 3.527161478886339*^9, 3.527164157789332*^9, 
   3.527166128365589*^9, 3.5271677739663057`*^9, 3.5271705149873466`*^9, 
   3.5271717923811007`*^9, {3.5271783147052298`*^9, 3.527178337995853*^9}, 
   3.527178468389876*^9, 3.527178521915209*^9, 3.527178719084285*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["Linearp", "COM"], "=", " ", 
  RowBox[{"Linearize", "[", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "COM"], "/.", "p0"}], ",", 
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{
       RowBox[{"Subscript", "[", 
        RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
    RowBox[{"Table", "[", 
     RowBox[{"0", ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}]], "Input",
 CellChangeTimes->{{3.5271612554748373`*^9, 3.527161273809671*^9}, {
  3.5271613215756617`*^9, 3.5271613259608994`*^9}, {3.5271614931981688`*^9, 
  3.5271614952899647`*^9}}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{
    RowBox[{"-", 
     RowBox[{"59809391653", " ", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "(", "t", ")"}]}]}], "-", 
    RowBox[{"17975220458", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], "+", 
    RowBox[{"7121032879", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], "-", 
    RowBox[{"7522240634", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], "-", 
    RowBox[{"701338095", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], "-", "8445660"}], 
   "122235500000"], TraditionalForm]], "Output",
 CellChangeTimes->{
  3.527161495746963*^9, 3.527164157907627*^9, 3.527166128451861*^9, 
   3.5271677740755873`*^9, 3.527170515224193*^9, 3.527171792518016*^9, {
   3.5271783198553257`*^9, 3.5271783421397543`*^9}, 3.5271784729359093`*^9, 
   3.5271785258194237`*^9, 3.527178719143724*^9}]
}, Open  ]],

Cell[BoxData[""], "Input",
 CellChangeTimes->{{3.515947985521303*^9, 3.515948032336985*^9}}],

Cell[CellGroupData[{

Cell[BoxData["p0"], "Input"],

Cell[BoxData[
 FormBox[
  RowBox[{"{", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["p", "x"], "(", "t", ")"}], "\[Rule]", "0"}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "z"], "(", "t", ")"}], "\[Rule]", "0"}], ",", 
    RowBox[{
     RowBox[{
      SuperscriptBox[
       SubscriptBox["p", "x"], "\[Prime]",
       MultilineFunction->None], "(", "t", ")"}], "\[Rule]", "0"}], ",", 
    RowBox[{
     RowBox[{
      SuperscriptBox[
       SubscriptBox["p", "z"], "\[Prime]",
       MultilineFunction->None], "(", "t", ")"}], "\[Rule]", "0"}]}], "}"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.52716151672077*^9, 3.527164157995164*^9, 
  3.527166128529793*^9, 3.5271677741560173`*^9, 3.5271705153229103`*^9, 
  3.527171792628175*^9, 3.527178719298499*^9}]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Write Outputs To disk", "Section",
 CellChangeTimes->{{3.527176889623287*^9, 3.5271768951406612`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<nstorsoFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"nstorso", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.52717692204714*^9, 3.5271769389718723`*^9}, {
  3.527177770383316*^9, 3.5271777843116817`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaNStorsoFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"LinearNStorso", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5271773510313377`*^9, 3.527177374463675*^9}, 
   3.527177789505682*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<storsoFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"storso", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.52717749547878*^9, 3.527177496241226*^9}, {
   3.527177526742309*^9, 3.52717753978264*^9}, {3.527177650458077*^9, 
   3.527177650554824*^9}, 3.527177794059635*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaStorsoFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"LinearStorso", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{3.527177499009471*^9, 3.527177543096356*^9, 
  3.5271777958436832`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<hipposFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"HipPos", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.527177507189144*^9, 3.5271775544415283`*^9}, 
   3.5271777971433496`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaHipposFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"LHipPos", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.527177561917758*^9, 3.527177576221059*^9}, 
   3.527177798533535*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<nsslopeFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"nsslope", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaNSslopeFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"LinearNSslope", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.527177585551565*^9, 3.5271776182148*^9}, {
  3.527177799901759*^9, 3.5271778041575823`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<comFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["p", "COM"], "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaCOMFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["Linearp", "COM"], " ", "/.", "p0"}], "/.", 
      "\[InvisibleSpace]", "constsubs"}], "/.", "statesubs"}]}], "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.527178155956102*^9, 3.527178183671091*^9}, {
  3.52717873969038*^9, 3.527178747613838*^9}}],

Cell[CellGroupData[{

Cell["PERL", "Subsection",
 CellChangeTimes->{{3.51353232609383*^9, 3.513532329542585*^9}}],

Cell["this next line assumes that perl is in your path", "Text",
 CellChangeTimes->{{3.513015230408543*^9, 3.513015241200741*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Run", "[", "\"\<perl math2mat_output.pl\>\"", 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build_torso\>\""}], 
   "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.504476276692404*^9, 3.504476301348216*^9}, {
   3.504478868125564*^9, 3.504478869967337*^9}, {3.504538702237328*^9, 
   3.504538704315194*^9}, {3.504631487786531*^9, 3.504631500259924*^9}, {
   3.504632214564033*^9, 3.504632215194798*^9}, {3.504635416937057*^9, 
   3.50463541912094*^9}, {3.504874639115547*^9, 3.504874639679282*^9}, 
   3.504876166859266*^9, {3.505578303460942*^9, 3.5055783039289427`*^9}, {
   3.512922890945364*^9, 3.512922891900902*^9}, {3.5129244660544987`*^9, 
   3.51292449464812*^9}, {3.512925468730114*^9, 3.512925469241001*^9}, {
   3.513011661780643*^9, 3.513011661926162*^9}, {3.51301497676734*^9, 
   3.513015001300451*^9}, {3.5158700422275343`*^9, 3.515870043613613*^9}, {
   3.515870412159693*^9, 3.5158704163519325`*^9}, {3.5158863672774124`*^9, 
   3.5158863722382207`*^9}, {3.5271773836362467`*^9, 3.527177386040779*^9}}],

Cell[BoxData[
 FormBox["0", TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5155188337118206`*^9, 3.5155213252703295`*^9, {3.515540878198961*^9, 
   3.515540903604343*^9}, 3.5158407423966546`*^9, 3.5158416340670977`*^9, 
   3.5158498815118246`*^9, 3.515850149364145*^9, 3.5158505826739287`*^9, 
   3.5158508174343567`*^9, 3.5158510975593786`*^9, 3.515851653032055*^9, 
   3.5158525885305624`*^9, 3.5158527509368515`*^9, 3.5158527931192636`*^9, 
   3.51585495715904*^9, 3.5158550282321053`*^9, 3.5158552191920276`*^9, 
   3.515855253120968*^9, 3.515865262254135*^9, 3.515865638210639*^9, 
   3.51586592793721*^9, 3.51587048063961*^9, 3.5158730153755884`*^9, 
   3.5158739792467184`*^9, 3.515874203634553*^9, 3.515886694531988*^9, 
   3.5158874913514147`*^9, 3.515889070384316*^9, 3.5158900577352266`*^9, 
   3.515890252205177*^9, 3.5159482968094587`*^9, 3.515948944374403*^9, 
   3.515953395315056*^9, 3.515953710496083*^9, 3.5159537761788397`*^9, 
   3.5159726585821743`*^9, 3.5159857720472217`*^9, 3.516707085463479*^9, 
   3.516709802924909*^9, 3.5167112898219547`*^9, 3.5167126251093287`*^9, 
   3.516731910384804*^9, 3.51675156425562*^9, 3.517679232673436*^9, 
   3.517679752897191*^9, 3.517680087541332*^9, 3.5176859974273577`*^9, 
   3.5176980466975365`*^9, 3.5176984843215675`*^9, 3.517698597905064*^9, 
   3.520109553737687*^9, 3.520120022112444*^9, 3.520177851995036*^9, 
   3.5201861307705555`*^9, 3.520256997752266*^9, 3.5202590894559045`*^9, 
   3.520339215071435*^9, 3.5204373594555745`*^9, 3.5204379012015605`*^9, 
   3.5204436639822655`*^9, 3.520449216190834*^9, 3.520456964627019*^9, 
   3.520532857654432*^9, 3.5205334050077386`*^9, 3.520533641289253*^9, 
   3.520700208631287*^9, 3.520713025182435*^9, 3.520715993050187*^9, 
   3.520716372698902*^9, 3.5207164419848647`*^9, 3.520720498096861*^9, 
   3.520734731372958*^9, 3.5207350240366974`*^9, 3.520735233941703*^9, 
   3.5207876835786457`*^9, 3.520788877843954*^9, 3.5208972402433395`*^9, 
   3.5213037296170855`*^9, 3.5213038482928734`*^9, 3.52148604821651*^9, 
   3.521487916633377*^9, 3.5222451196790495`*^9, 3.522246794946869*^9, 
   3.52224990060228*^9, 3.522330715643431*^9, 3.5223566127951155`*^9, 
   3.5223569178824873`*^9, 3.5224296564487324`*^9, 3.522510361860586*^9, 
   3.5225118733730397`*^9, 3.522708144818318*^9, 3.5227751575219574`*^9, 
   3.522776845738859*^9, 3.5230285840646763`*^9, 3.5230306390121126`*^9, 
   3.523031339818296*^9, 3.5235572703774643`*^9, 3.52372487579274*^9, 
   3.5237265671694813`*^9, 3.5239837459472656`*^9, 3.5239849930135937`*^9, 
   3.5239893079543943`*^9, 3.5239903317719536`*^9, 3.523992025216813*^9, 
   3.5239921368791995`*^9, 3.5239948186215863`*^9, 3.5239949284198666`*^9, 
   3.5239950392682066`*^9, 3.5242481412858143`*^9, 3.524248528452959*^9, 
   3.5242499977068315`*^9, 3.5244001128388147`*^9, 3.5244002976151495`*^9, 
   3.5244062695321836`*^9, 3.524406550370155*^9, 3.5244067850253863`*^9, 
   3.524416267410578*^9, 3.524417771154109*^9, 3.5244178686872463`*^9, 
   3.5244183936479816`*^9, 3.524418700642165*^9, 3.524422224071623*^9, 
   3.5244223112816105`*^9, 3.524422752104824*^9, 3.5244238419211583`*^9, 
   3.524442216849144*^9, 3.524442742387203*^9, 3.5244438493725195`*^9, 
   3.5244456864925966`*^9, 3.5244458645247793`*^9, 3.5244468702188396`*^9, 
   3.5245024335606823`*^9, 3.5245119471381826`*^9, 3.5245883990742893`*^9, 
   3.525090287048571*^9, 3.525091695188668*^9, 3.5250919519797196`*^9, 
   3.5250921900848627`*^9, 3.5250924453299904`*^9, 3.525092571060454*^9, 
   3.52509268956332*^9, 3.5250928935899105`*^9, 3.5250932069259415`*^9, 
   3.525098766927955*^9, 3.5254479897919855`*^9, 3.525461216302328*^9, 
   3.5255182690806684`*^9, 3.525824548871134*^9, 3.527177429714081*^9, 
   3.527177710324808*^9}]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["feedback control", "Section",
 CellChangeTimes->{{3.513532015025627*^9, 3.513532018772655*^9}}],

Cell["we will make use of the chain rule as much as possible", "Text",
 CellChangeTimes->{{3.513534246341532*^9, 3.513534256526356*^9}, {
  3.513534308611278*^9, 3.513534359658739*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"\[Chi]", "=", 
   RowBox[{"Join", "[", 
    RowBox[{"q", ",", "dq"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"d\[Chi]", "=", 
   RowBox[{"D", "[", 
    RowBox[{"\[Chi]", ",", "t"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533323577435*^9, 3.513533331092033*^9}, {
  3.513533381181343*^9, 3.513533384041068*^9}, {3.513534180527118*^9, 
  3.513534186497912*^9}, {3.513534242697953*^9, 3.513534243136149*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  SubscriptBox["p", "hip"], " ", "=", " ", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], "\[LeftDoubleBracket]", 
    RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
   "p0"}]}], "\[IndentingNewLine]", 
 RowBox[{
  SubscriptBox["p", "hipdot"], "  ", "=", " ", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    SubscriptBox["\[PartialD]", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
    RowBox[{"Flatten", "[", 
     SubscriptBox["p", "hip"], "]"}]}], "]"}]}]}], "Input",
 CellChangeTimes->{{3.520119821955995*^9, 3.5201198226710362`*^9}, {
  3.5201773456230736`*^9, 3.52017734590409*^9}}],

Cell[BoxData[
 RowBox[{"-", 
  FractionBox[
   RowBox[{"127", " ", 
    RowBox[{"(", 
     RowBox[{
      RowBox[{"685", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
      RowBox[{"514", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
     ")"}]}], "250000"]}]], "Output",
 CellChangeTimes->{
  3.5201199730386367`*^9, 3.520177360535927*^9, 3.5201777980259495`*^9, 
   3.5201860777815247`*^9, 3.520256945760292*^9, 3.5202590327286596`*^9, 
   3.520339159378249*^9, 3.5204373045234327`*^9, 3.5204378432872477`*^9, 
   3.520443601169673*^9, 3.520449157358469*^9, 3.520456904194563*^9, 
   3.520532793944788*^9, 3.5205335824788895`*^9, 3.5207001269116135`*^9, 
   3.52071296984527*^9, 3.520715988566931*^9, 3.5207163683716545`*^9, 
   3.5207204914814825`*^9, 3.5207347256446304`*^9, 3.520735229659458*^9, 
   3.520787678905379*^9, 3.520788873494705*^9, 3.520897235076044*^9, 
   3.5213037236587443`*^9, 3.5213038433835926`*^9, 3.5214860430502143`*^9, 
   3.5214879122801285`*^9, 3.522245115748824*^9, 3.5222456811611643`*^9, 
   3.522246790846635*^9, 3.522249895776004*^9, 3.522330706842928*^9, 
   3.522356608473908*^9, 3.5223569135736256`*^9, 3.5224296516144557`*^9, 
   3.522510255287491*^9, 3.5225118670036755`*^9, 3.5227081356777954`*^9, 
   3.5227751530147*^9, 3.522776840231544*^9, 3.5230285783883514`*^9, 
   3.5230306328517723`*^9, 3.523031332737891*^9, 3.5235572655271873`*^9, 
   3.523724872241537*^9, 3.523726563804289*^9, 3.523983742318058*^9, 
   3.5239849896093993`*^9, 3.523989304450194*^9, 3.523990328335757*^9, 
   3.5239920217986174`*^9, 3.523992133427002*^9, 3.523994815205391*^9, 
   3.5239949249706693`*^9, 3.523995035853011*^9, 3.5242481378856196`*^9, 
   3.5242485250027614`*^9, 3.524249991771492*^9, 3.52440010950881*^9, 
   3.5244002941199493`*^9, 3.5244062660389867`*^9, 3.5244065469499617`*^9, 
   3.5244067816041946`*^9, 3.524407861553268*^9, 3.5244154479797087`*^9, 
   3.5244155730138607`*^9, 3.5244158493786674`*^9, 3.5244159463952165`*^9, 
   3.5244179099173045`*^9, 3.524418035767481*^9, 3.524418284227829*^9, 
   3.524418390317978*^9, 3.52441869740598*^9, 3.524422220551421*^9, 
   3.5244223078284135`*^9, 3.5244227488736396`*^9, 3.5244238384909625`*^9, 
   3.5244422135149536`*^9, 3.524442738970008*^9, 3.52444384588632*^9, 
   3.524445683010397*^9, 3.5244458610695815`*^9, 3.5244468654705687`*^9, 
   3.524502428193375*^9, 3.524511941294848*^9, 3.524588395626092*^9, 
   3.525090283610377*^9, 3.525091691740476*^9, 3.5250919485885754`*^9, 
   3.5250921866816945`*^9, 3.5250924418857956`*^9, 3.525092567613266*^9, 
   3.525092685992117*^9, 3.525092890155772*^9, 3.5250932019776583`*^9, 
   3.525098762024675*^9, 3.5254479853897333`*^9, {3.5254611901868343`*^9, 
   3.52546121283313*^9}, 3.5255182638563695`*^9, 3.5258244786911197`*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"-", 
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"685", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}], "250000"]}], ",", 
   RowBox[{"-", 
    FractionBox[
     RowBox[{"32639", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], 
     "125000"]}], ",", "0", ",", "0", ",", "0"}], "}"}]], "Output",
 CellChangeTimes->{
  3.5201199730386367`*^9, 3.520177360535927*^9, 3.5201777980259495`*^9, 
   3.5201860777815247`*^9, 3.520256945760292*^9, 3.5202590327286596`*^9, 
   3.520339159378249*^9, 3.5204373045234327`*^9, 3.5204378432872477`*^9, 
   3.520443601169673*^9, 3.520449157358469*^9, 3.520456904194563*^9, 
   3.520532793944788*^9, 3.5205335824788895`*^9, 3.5207001269116135`*^9, 
   3.52071296984527*^9, 3.520715988566931*^9, 3.5207163683716545`*^9, 
   3.5207204914814825`*^9, 3.5207347256446304`*^9, 3.520735229659458*^9, 
   3.520787678905379*^9, 3.520788873494705*^9, 3.520897235076044*^9, 
   3.5213037236587443`*^9, 3.5213038433835926`*^9, 3.5214860430502143`*^9, 
   3.5214879122801285`*^9, 3.522245115748824*^9, 3.5222456811611643`*^9, 
   3.522246790846635*^9, 3.522249895776004*^9, 3.522330706842928*^9, 
   3.522356608473908*^9, 3.5223569135736256`*^9, 3.5224296516144557`*^9, 
   3.522510255287491*^9, 3.5225118670036755`*^9, 3.5227081356777954`*^9, 
   3.5227751530147*^9, 3.522776840231544*^9, 3.5230285783883514`*^9, 
   3.5230306328517723`*^9, 3.523031332737891*^9, 3.5235572655271873`*^9, 
   3.523724872241537*^9, 3.523726563804289*^9, 3.523983742318058*^9, 
   3.5239849896093993`*^9, 3.523989304450194*^9, 3.523990328335757*^9, 
   3.5239920217986174`*^9, 3.523992133427002*^9, 3.523994815205391*^9, 
   3.5239949249706693`*^9, 3.523995035853011*^9, 3.5242481378856196`*^9, 
   3.5242485250027614`*^9, 3.524249991771492*^9, 3.52440010950881*^9, 
   3.5244002941199493`*^9, 3.5244062660389867`*^9, 3.5244065469499617`*^9, 
   3.5244067816041946`*^9, 3.524407861553268*^9, 3.5244154479797087`*^9, 
   3.5244155730138607`*^9, 3.5244158493786674`*^9, 3.5244159463952165`*^9, 
   3.5244179099173045`*^9, 3.524418035767481*^9, 3.524418284227829*^9, 
   3.524418390317978*^9, 3.52441869740598*^9, 3.524422220551421*^9, 
   3.5244223078284135`*^9, 3.5244227488736396`*^9, 3.5244238384909625`*^9, 
   3.5244422135149536`*^9, 3.524442738970008*^9, 3.52444384588632*^9, 
   3.524445683010397*^9, 3.5244458610695815`*^9, 3.5244468654705687`*^9, 
   3.524502428193375*^9, 3.524511941294848*^9, 3.524588395626092*^9, 
   3.525090283610377*^9, 3.525091691740476*^9, 3.5250919485885754`*^9, 
   3.5250921866816945`*^9, 3.5250924418857956`*^9, 3.525092567613266*^9, 
   3.525092685992117*^9, 3.525092890155772*^9, 3.5250932019776583`*^9, 
   3.525098762024675*^9, 3.5254479853897333`*^9, {3.5254611901868343`*^9, 
   3.52546121283313*^9}, 3.5255182638563695`*^9, 3.5258244787081203`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"hipOutput", " ", "=", " ", 
   RowBox[{"{", 
    RowBox[{"HipPos", ",", "LHipPos", ",", "LHipPosD"}], "}"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["deltap", "hip"], " ", "=", 
   RowBox[{"hipOutput", "[", 
    RowBox[{"[", "2", "]"}], "]"}]}], " "}], "\[IndentingNewLine]", 
 RowBox[{
  SubscriptBox["deltap", "hipdot"], "  ", "=", " ", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    SubscriptBox["\[PartialD]", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
    RowBox[{"Flatten", "[", 
     SubscriptBox["deltap", "hip"], "]"}]}], "]"}]}], "\[IndentingNewLine]", 
 RowBox[{
  SubscriptBox["deltav", "hip"], "=", 
  RowBox[{
   RowBox[{"D", "[", 
    RowBox[{
     SubscriptBox["deltap", "hip"], ",", "t"}], "]"}], "//", 
   "Simplify"}]}]}], "Input",
 CellChangeTimes->{{3.513532470067184*^9, 3.513532510398695*^9}, {
   3.513532641208467*^9, 3.51353265065868*^9}, {3.513532881357095*^9, 
   3.513532897241124*^9}, {3.513887124348741*^9, 3.513887126504996*^9}, {
   3.5158548964365673`*^9, 3.515854897203611*^9}, 3.5158587723352556`*^9, 
   3.5158641632552757`*^9, {3.520707811439011*^9, 3.5207079217130823`*^9}, {
   3.5207152255042863`*^9, 3.520715225775302*^9}, {3.5207156395659695`*^9, 
   3.52071567122478*^9}, {3.520715718012456*^9, 3.520715733202325*^9}, {
   3.520715881236792*^9, 3.5207158951315866`*^9}, {3.5207160492264004`*^9, 
   3.520716069953586*^9}, {3.520735000946377*^9, 3.520735001317398*^9}, {
   3.522510212466041*^9, 3.5225102129260674`*^9}, 3.522511833984787*^9, {
   3.5258243167418566`*^9, 3.525824317556903*^9}, 3.526949598410554*^9}],

Cell[BoxData[
 RowBox[{"-", 
  FractionBox[
   RowBox[{"127", " ", 
    RowBox[{"(", 
     RowBox[{
      RowBox[{"685", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
      RowBox[{"514", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
     ")"}]}], "250000"]}]], "Output",
 CellChangeTimes->{
  3.520715722161693*^9, 3.520715988625934*^9, {3.5207160506364813`*^9, 
   3.5207160708336363`*^9}, 3.5207163684236574`*^9, 3.520720491525485*^9, 
   3.520734725695633*^9, 3.520735004052554*^9, 3.520735229723462*^9, 
   3.520787678959382*^9, 3.5207888735457087`*^9, 3.520897235141048*^9, 
   3.5213037237077475`*^9, 3.5213038435015993`*^9, 3.521486043112218*^9, 
   3.5214879123341312`*^9, 3.5222451157968273`*^9, 3.5222456812261677`*^9, 
   3.5222467909106383`*^9, 3.522249895862009*^9, 3.522330706903931*^9, 
   3.522356608520708*^9, 3.5223569136516256`*^9, 3.522429651649458*^9, 
   3.5225102553834963`*^9, 3.522511867199687*^9, 3.5227081357698*^9, 
   3.5227751530457015`*^9, 3.5227768402935476`*^9, 3.523028578593363*^9, 
   3.523030632896775*^9, 3.5230313328218956`*^9, 3.5235572655771894`*^9, 
   3.5237248722835393`*^9, 3.52372656382929*^9, 3.5239837423450594`*^9, 
   3.5239849896314*^9, 3.523989304473195*^9, 3.5239903283697586`*^9, 
   3.523992021830619*^9, 3.523992133453004*^9, 3.523994815234393*^9, 
   3.523994925000671*^9, 3.523995035884013*^9, 3.5242481379106207`*^9, 
   3.5242485250417633`*^9, 3.5242499918364954`*^9, 3.52440010952881*^9, 
   3.52440029414295*^9, 3.5244062660679884`*^9, 3.5244065469849634`*^9, 
   3.5244067816401963`*^9, 3.5244078615962706`*^9, 3.52441544800071*^9, 
   3.5244155730388613`*^9, 3.5244158494146695`*^9, 3.524415946416218*^9, 
   3.5244179099473047`*^9, 3.5244180357974806`*^9, 3.524418297897848*^9, 
   3.5244183903679776`*^9, 3.5244186974189806`*^9, 3.524422220583423*^9, 
   3.524422307864415*^9, 3.5244227488876405`*^9, 3.5244238385069633`*^9, 
   3.524442213541955*^9, 3.52444273901101*^9, 3.524443845921322*^9, 
   3.5244456830473995`*^9, 3.5244458611015835`*^9, 3.5244468655275717`*^9, 
   3.5245024282523785`*^9, 3.524511941364852*^9, 3.5245883956510935`*^9, 
   3.5250902836403785`*^9, 3.5250916917764783`*^9, 3.525091948628576*^9, 
   3.525092186712696*^9, 3.525092441915797*^9, 3.5250925676482677`*^9, 
   3.5250926860421195`*^9, 3.525092890189774*^9, 3.5250932020326614`*^9, 
   3.525098762076678*^9, 3.525447985424735*^9, {3.5254611902128363`*^9, 
   3.5254612128651314`*^9}, 3.5255182638913717`*^9, 3.5258244787241216`*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"-", 
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"685", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}], "250000"]}], ",", 
   RowBox[{"-", 
    FractionBox[
     RowBox[{"32639", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], 
     "125000"]}], ",", "0", ",", "0", ",", "0"}], "}"}]], "Output",
 CellChangeTimes->{
  3.520715722161693*^9, 3.520715988625934*^9, {3.5207160506364813`*^9, 
   3.5207160708336363`*^9}, 3.5207163684236574`*^9, 3.520720491525485*^9, 
   3.520734725695633*^9, 3.520735004052554*^9, 3.520735229723462*^9, 
   3.520787678959382*^9, 3.5207888735457087`*^9, 3.520897235141048*^9, 
   3.5213037237077475`*^9, 3.5213038435015993`*^9, 3.521486043112218*^9, 
   3.5214879123341312`*^9, 3.5222451157968273`*^9, 3.5222456812261677`*^9, 
   3.5222467909106383`*^9, 3.522249895862009*^9, 3.522330706903931*^9, 
   3.522356608520708*^9, 3.5223569136516256`*^9, 3.522429651649458*^9, 
   3.5225102553834963`*^9, 3.522511867199687*^9, 3.5227081357698*^9, 
   3.5227751530457015`*^9, 3.5227768402935476`*^9, 3.523028578593363*^9, 
   3.523030632896775*^9, 3.5230313328218956`*^9, 3.5235572655771894`*^9, 
   3.5237248722835393`*^9, 3.52372656382929*^9, 3.5239837423450594`*^9, 
   3.5239849896314*^9, 3.523989304473195*^9, 3.5239903283697586`*^9, 
   3.523992021830619*^9, 3.523992133453004*^9, 3.523994815234393*^9, 
   3.523994925000671*^9, 3.523995035884013*^9, 3.5242481379106207`*^9, 
   3.5242485250417633`*^9, 3.5242499918364954`*^9, 3.52440010952881*^9, 
   3.52440029414295*^9, 3.5244062660679884`*^9, 3.5244065469849634`*^9, 
   3.5244067816401963`*^9, 3.5244078615962706`*^9, 3.52441544800071*^9, 
   3.5244155730388613`*^9, 3.5244158494146695`*^9, 3.524415946416218*^9, 
   3.5244179099473047`*^9, 3.5244180357974806`*^9, 3.524418297897848*^9, 
   3.5244183903679776`*^9, 3.5244186974189806`*^9, 3.524422220583423*^9, 
   3.524422307864415*^9, 3.5244227488876405`*^9, 3.5244238385069633`*^9, 
   3.524442213541955*^9, 3.52444273901101*^9, 3.524443845921322*^9, 
   3.5244456830473995`*^9, 3.5244458611015835`*^9, 3.5244468655275717`*^9, 
   3.5245024282523785`*^9, 3.524511941364852*^9, 3.5245883956510935`*^9, 
   3.5250902836403785`*^9, 3.5250916917764783`*^9, 3.525091948628576*^9, 
   3.525092186712696*^9, 3.525092441915797*^9, 3.5250925676482677`*^9, 
   3.5250926860421195`*^9, 3.525092890189774*^9, 3.5250932020326614`*^9, 
   3.525098762076678*^9, 3.525447985424735*^9, {3.5254611902128363`*^9, 
   3.5254612128651314`*^9}, 3.5255182638913717`*^9, 3.5258244787371225`*^9}],

Cell[BoxData[
 RowBox[{"-", 
  FractionBox[
   RowBox[{"127", " ", 
    RowBox[{"(", 
     RowBox[{
      RowBox[{"685", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], " ", 
       RowBox[{
        SuperscriptBox[
         SubscriptBox["\[Theta]", "1"], "\[Prime]",
         MultilineFunction->None], "[", "t", "]"}]}], "+", 
      RowBox[{"514", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "1"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "+", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "2"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], 
   "250000"]}]], "Output",
 CellChangeTimes->{
  3.520715722161693*^9, 3.520715988625934*^9, {3.5207160506364813`*^9, 
   3.5207160708336363`*^9}, 3.5207163684236574`*^9, 3.520720491525485*^9, 
   3.520734725695633*^9, 3.520735004052554*^9, 3.520735229723462*^9, 
   3.520787678959382*^9, 3.5207888735457087`*^9, 3.520897235141048*^9, 
   3.5213037237077475`*^9, 3.5213038435015993`*^9, 3.521486043112218*^9, 
   3.5214879123341312`*^9, 3.5222451157968273`*^9, 3.5222456812261677`*^9, 
   3.5222467909106383`*^9, 3.522249895862009*^9, 3.522330706903931*^9, 
   3.522356608520708*^9, 3.5223569136516256`*^9, 3.522429651649458*^9, 
   3.5225102553834963`*^9, 3.522511867199687*^9, 3.5227081357698*^9, 
   3.5227751530457015`*^9, 3.5227768402935476`*^9, 3.523028578593363*^9, 
   3.523030632896775*^9, 3.5230313328218956`*^9, 3.5235572655771894`*^9, 
   3.5237248722835393`*^9, 3.52372656382929*^9, 3.5239837423450594`*^9, 
   3.5239849896314*^9, 3.523989304473195*^9, 3.5239903283697586`*^9, 
   3.523992021830619*^9, 3.523992133453004*^9, 3.523994815234393*^9, 
   3.523994925000671*^9, 3.523995035884013*^9, 3.5242481379106207`*^9, 
   3.5242485250417633`*^9, 3.5242499918364954`*^9, 3.52440010952881*^9, 
   3.52440029414295*^9, 3.5244062660679884`*^9, 3.5244065469849634`*^9, 
   3.5244067816401963`*^9, 3.5244078615962706`*^9, 3.52441544800071*^9, 
   3.5244155730388613`*^9, 3.5244158494146695`*^9, 3.524415946416218*^9, 
   3.5244179099473047`*^9, 3.5244180357974806`*^9, 3.524418297897848*^9, 
   3.5244183903679776`*^9, 3.5244186974189806`*^9, 3.524422220583423*^9, 
   3.524422307864415*^9, 3.5244227488876405`*^9, 3.5244238385069633`*^9, 
   3.524442213541955*^9, 3.52444273901101*^9, 3.524443845921322*^9, 
   3.5244456830473995`*^9, 3.5244458611015835`*^9, 3.5244468655275717`*^9, 
   3.5245024282523785`*^9, 3.524511941364852*^9, 3.5245883956510935`*^9, 
   3.5250902836403785`*^9, 3.5250916917764783`*^9, 3.525091948628576*^9, 
   3.525092186712696*^9, 3.525092441915797*^9, 3.5250925676482677`*^9, 
   3.5250926860421195`*^9, 3.525092890189774*^9, 3.5250932020326614`*^9, 
   3.525098762076678*^9, 3.525447985424735*^9, {3.5254611902128363`*^9, 
   3.5254612128651314`*^9}, 3.5255182638913717`*^9, 3.5258244787891254`*^9}]
}, Open  ]],

Cell["time-invariant parameterization:", "Text",
 CellChangeTimes->{{3.513532550684887*^9, 3.513532556202501*^9}, {
  3.513532644709088*^9, 3.513532645133727*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"fb", "=", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
    FractionBox[
     RowBox[{
      SubscriptBox["deltap", "hip"], "-", 
      RowBox[{"p", "[", "1", "]"}]}], 
     RowBox[{"a", "[", 
      RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}]], "Input",
 CellChangeTimes->{{3.513532403934621*^9, 3.513532467910504*^9}, {
   3.513532514383117*^9, 3.513532544860577*^9}, {3.51353289142399*^9, 
   3.513532893553784*^9}, {3.513533036340894*^9, 3.513533036810536*^9}, {
   3.513533928303517*^9, 3.513533928507984*^9}, {3.513534006139525*^9, 
   3.513534007303687*^9}, {3.51394646342299*^9, 3.513946465979686*^9}, {
   3.513946518081392*^9, 3.513946518637576*^9}, {3.515336393379112*^9, 
   3.515336393434178*^9}, {3.5158551745864763`*^9, 3.515855177569647*^9}, 
   3.5201773758658037`*^9, 3.520186024265464*^9, 3.520443576003234*^9, 
   3.5204490394407244`*^9, 3.5207126311878996`*^9, 3.5223564440908136`*^9, 
   3.5224291342898664`*^9, 3.5244183182878766`*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
   FractionBox[
    RowBox[{
     RowBox[{"-", 
      RowBox[{"p", "[", "1", "]"}]}], "-", 
     FractionBox[
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"685", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"514", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}]}], "250000"]}], 
    RowBox[{"a", "[", 
     RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]], "Output",
 CellChangeTimes->{
  3.524418318767877*^9, 3.5244183904179773`*^9, 3.5244186974239807`*^9, 
   3.5244222206114244`*^9, 3.524422307896417*^9, 3.5244227488936405`*^9, 
   3.5244238385129633`*^9, 3.5244422135569563`*^9, 3.524442739039012*^9, 
   3.5244438459483232`*^9, 3.524445683077401*^9, 3.524445861127585*^9, 
   3.524446865566574*^9, 3.524502428333383*^9, 3.524511941428856*^9, 
   3.524588395669094*^9, 3.5250902836633797`*^9, 3.5250916918124804`*^9, 
   3.525091948668576*^9, 3.5250921867346973`*^9, 3.5250924419427986`*^9, 
   3.52509256767727*^9, 3.525092686086122*^9, 3.5250928902067747`*^9, 
   3.525093202075664*^9, 3.52509876211268*^9, 3.5254479854617376`*^9, {
   3.525461190235837*^9, 3.5254612128871326`*^9}, 3.525518263916373*^9, 
   3.525824478804126*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   RowBox[{"fb", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
      FractionBox[
       RowBox[{
        SubscriptBox["deltap", "hip"], "-", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "2"}], "]"}]}], 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";"}], "*)"}]], "Input",
 CellChangeTimes->{{3.520186027790665*^9, 3.520186030061795*^9}, 
   3.520443571808994*^9, 3.520449035767514*^9, 3.520712638343309*^9, 
   3.5223564391349745`*^9, 3.522429124853327*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"\[Sigma]y", "=", 
  RowBox[{
   RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "fb"}]}]], "Input",
 CellChangeTimes->{{3.5158620968964095`*^9, 3.5158621015066733`*^9}, {
   3.51586213624666*^9, 3.515862144043106*^9}, 3.5158641571459265`*^9}],

Cell[BoxData[
 FractionBox[
  RowBox[{
   RowBox[{"-", 
    RowBox[{"p", "[", "1", "]"}]}], "-", 
   FractionBox[
    RowBox[{"127", " ", 
     RowBox[{"(", 
      RowBox[{
       RowBox[{"685", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
       RowBox[{"514", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
      ")"}]}], "250000"]}], 
  RowBox[{"a", "[", 
   RowBox[{"1", ",", "1"}], "]"}]]], "Output",
 CellChangeTimes->{
  3.5158641792271895`*^9, 3.515864640138552*^9, 3.5158657553733397`*^9, 
   3.5158658130416384`*^9, 3.515870350322156*^9, 3.515870394940708*^9, 
   3.515872932375841*^9, 3.515886480796013*^9, 3.5158865929134097`*^9, 
   3.5158872951030703`*^9, 3.5158873858328295`*^9, 3.515888965224531*^9, 
   3.515889950650238*^9, 3.515890141062775*^9, 3.515948192237275*^9, 
   3.515948740957839*^9, 3.5159488339652023`*^9, 3.515953274969172*^9, 
   3.515953597921644*^9, 3.5159725664689054`*^9, 3.5159856851012487`*^9, 
   3.516706980522477*^9, 3.516709724139403*^9, 3.5167112131405687`*^9, 
   3.516712549888026*^9, 3.5167318368765993`*^9, 3.5167514947046423`*^9, 
   3.517679155733035*^9, 3.5176795889938164`*^9, 3.5176796785299377`*^9, 
   3.517680011475981*^9, 3.517685923424125*^9, 3.5176979716302433`*^9, 
   3.5176984092622747`*^9, 3.517698523319798*^9, 3.520109483076645*^9, 
   3.520119973168644*^9, 3.520177798310966*^9, 3.5201860781275444`*^9, 
   3.520256946061309*^9, 3.5202590329686737`*^9, 3.5203391599152803`*^9, 
   3.5204373052254725`*^9, 3.5204378434762588`*^9, 3.520443601406687*^9, 
   3.520449157600483*^9, 3.5204569043695726`*^9, 3.520532794415815*^9, 
   3.5205335826579*^9, 3.520700127396641*^9, 3.520712970163288*^9, 
   3.5207159886939383`*^9, 3.5207163684716597`*^9, 3.520720491571488*^9, 
   3.5207347257416363`*^9, 3.5207352297914658`*^9, 3.520787679063388*^9, 
   3.5207888736047115`*^9, 3.5208972352170525`*^9, 3.5213037239687624`*^9, 
   3.521303843652608*^9, 3.521486043195223*^9, 3.5214879124011354`*^9, 
   3.5222451158618307`*^9, 3.522245681329174*^9, 3.5222467910076437`*^9, 
   3.522249895957014*^9, 3.5223307069849358`*^9, 3.5223566085831084`*^9, 
   3.522356913745226*^9, 3.522429651671459*^9, 3.5225102555695066`*^9, 
   3.5225118673926973`*^9, 3.5227081365208435`*^9, 3.5227751530827036`*^9, 
   3.5227768403775525`*^9, 3.5230285786753674`*^9, 3.5230306329327765`*^9, 
   3.523031333017907*^9, 3.5235572656221924`*^9, 3.523724872332542*^9, 
   3.5237265638572917`*^9, 3.5239837424640665`*^9, 3.523984989654402*^9, 
   3.5239893044971967`*^9, 3.523990328409761*^9, 3.5239920218576207`*^9, 
   3.5239921334760046`*^9, 3.5239948152643943`*^9, 3.523994925037673*^9, 
   3.5239950359180155`*^9, 3.5242481379376225`*^9, 3.524248525081766*^9, 
   3.5242499919245005`*^9, 3.52440010954881*^9, 3.5244002941769524`*^9, 
   3.524406266135992*^9, 3.5244065470219655`*^9, 3.524406781674198*^9, 
   3.5244078616392727`*^9, 3.524415448024711*^9, 3.524415573061863*^9, 
   3.5244158494426713`*^9, 3.524415946438219*^9, 3.5244179099773045`*^9, 
   3.5244180358374805`*^9, 3.524418390437978*^9, 3.524418697426981*^9, 
   3.5244222206544275`*^9, 3.5244223079294195`*^9, 3.524422748899641*^9, 
   3.524423838517964*^9, 3.5244422135739565`*^9, 3.524442739068013*^9, 
   3.5244438459753246`*^9, 3.5244456831164036`*^9, 3.5244458611585865`*^9, 
   3.524446865604576*^9, 3.5245024285493956`*^9, 3.524511941464858*^9, 
   3.5245883956870956`*^9, 3.5250902836973815`*^9, 3.525091691844482*^9, 
   3.525091948708576*^9, 3.525092186761699*^9, 3.5250924419698005`*^9, 
   3.525092567707271*^9, 3.525092686133125*^9, 3.525092890228776*^9, 
   3.5250932021276665`*^9, 3.5250987621416817`*^9, 3.5254479854827385`*^9, {
   3.5254611902598386`*^9, 3.525461212912134*^9}, 3.525518263942374*^9, 
   3.5258244788231273`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[TextData[{
 "calculate ",
 Cell[BoxData[
  FormBox[
   SubscriptBox["y", "d"], TraditionalForm]]],
 " and its derivatives"
}], "Subsubsection",
 CellChangeTimes->{{3.513534419031216*^9, 3.513534436458221*^9}, {
  3.515862063545502*^9, 3.515862065419609*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"HumanFunction", "[", "i_", "]"}], ":=", 
   RowBox[{
    FractionBox[
     RowBox[{
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"i", ",", "1"}], "]"}], " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "2"}], "]"}], " ", 
         RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"i", ",", "3"}], "]"}], " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "2"}], "]"}], " ", 
         RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}]}], 
     RowBox[{"Exp", "[", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"i", ",", "4"}], "]"}], 
       RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]], "+", 
    RowBox[{"a", "[", 
     RowBox[{"i", ",", "5"}], "]"}]}]}], ";"}]], "Input",
 CellChangeTimes->{{3.515860352077612*^9, 3.5158603885236964`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  SubscriptBox["y", 
   RowBox[{"d", ",", "1"}]], " ", "=", " ", 
  RowBox[{"a", "[", 
   RowBox[{"1", ",", "1"}], "]"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["y", 
    RowBox[{"d", ",", "2"}]], " ", "=", " ", 
   RowBox[{
    RowBox[{"Transpose", "[", 
     RowBox[{"{", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", " ", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
     "]"}], "/.", "fb"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.515872287951982*^9, 3.5158723015107574`*^9}, {
   3.5158723371567965`*^9, 3.5158723885397353`*^9}, {3.515886208937134*^9, 
   3.51588621787595*^9}, {3.5158865763305807`*^9, 3.515886577094982*^9}, {
   3.516754046543599*^9, 3.5167540468006134`*^9}, 3.52441582511928*^9}],

Cell[BoxData[
 RowBox[{"a", "[", 
  RowBox[{"1", ",", "1"}], "]"}]], "Output",
 CellChangeTimes->{
  3.515872388987761*^9, 3.5158729324648457`*^9, 3.5158864808584127`*^9, 
   3.51588659300701*^9, 3.5158872951810703`*^9, 3.5158873859108295`*^9, 
   3.5158889653025317`*^9, 3.5158899507438383`*^9, 3.515890141140775*^9, 
   3.5159481922684746`*^9, 3.515948741004639*^9, 3.5159488340276027`*^9, 
   3.5159532750331755`*^9, 3.5159535980216494`*^9, 3.51597256654491*^9, 
   3.5159856851642523`*^9, 3.516706980589481*^9, 3.516709724218407*^9, 
   3.5167112132235737`*^9, 3.5167125499910326`*^9, 3.516731836973605*^9, 
   3.5167514948486505`*^9, 3.516754052551943*^9, 3.517679155782038*^9, 
   3.517679589035819*^9, 3.517679678596942*^9, 3.5176800115139832`*^9, 
   3.517685923475128*^9, 3.517697971677246*^9, 3.517698409306277*^9, 
   3.5176985233477993`*^9, 3.5201094831326485`*^9, 3.520119973204646*^9, 
   3.5201777983559685`*^9, 3.5201860781755466`*^9, 3.520256946096311*^9, 
   3.5202590330126762`*^9, 3.520339159947282*^9, 3.520437305276476*^9, 
   3.5204378435052605`*^9, 3.5204436014536896`*^9, 3.52044915772149*^9, 
   3.5204569044005747`*^9, 3.5205327944498167`*^9, 3.520533582695902*^9, 
   3.5207001274386435`*^9, 3.5207129701882896`*^9, 3.5207159887319403`*^9, 
   3.520716368504662*^9, 3.5207204915964894`*^9, 3.5207347257746377`*^9, 
   3.5207352298284683`*^9, 3.5207876791073904`*^9, 3.5207888736347136`*^9, 
   3.5208972352650547`*^9, 3.5213037240957694`*^9, 3.521303843733612*^9, 
   3.5214860432392254`*^9, 3.521487912451138*^9, 3.522245115897833*^9, 
   3.522245681382177*^9, 3.5222467910616465`*^9, 3.5222498960080175`*^9, 
   3.5223307071159434`*^9, 3.5223566086143084`*^9, 3.522356913776426*^9, 
   3.5224296516814594`*^9, 3.522510255703514*^9, 3.5225118674757023`*^9, 
   3.5227081370118713`*^9, 3.5227751531137056`*^9, 3.5227768404495564`*^9, 
   3.5230285787583723`*^9, 3.5230306330127816`*^9, 3.5230313330959115`*^9, 
   3.523557265664195*^9, 3.523724872400546*^9, 3.523726563883293*^9, 
   3.5239837424870677`*^9, 3.5239849896754026`*^9, 3.5239893045151978`*^9, 
   3.523990328441763*^9, 3.5239920218886223`*^9, 3.523992133501006*^9, 
   3.523994815298396*^9, 3.523994925070675*^9, 3.5239950359440165`*^9, 
   3.524248137996626*^9, 3.5242485251137676`*^9, 3.524249992034507*^9, 
   3.5244001095788097`*^9, 3.5244002942149544`*^9, 3.524406266172994*^9, 
   3.5244065470539675`*^9, 3.5244067817052*^9, 3.524407861680275*^9, 
   3.5244154480767145`*^9, 3.524415573094865*^9, 3.5244158494826736`*^9, 
   3.5244159464602203`*^9, 3.5244179100473046`*^9, 3.5244180358774805`*^9, 
   3.5244183904679775`*^9, 3.5244186974319816`*^9, 3.524422220712431*^9, 
   3.524422307970422*^9, 3.524422748915642*^9, 3.5244238385249643`*^9, 
   3.524442213590958*^9, 3.5244427390980153`*^9, 3.5244438460053263`*^9, 
   3.524445683165406*^9, 3.5244458611855884`*^9, 3.5244468656405783`*^9, 
   3.524502428583398*^9, 3.52451194150386*^9, 3.5245883957050962`*^9, 
   3.525090283747384*^9, 3.5250916918774843`*^9, 3.525091948738576*^9, 
   3.5250921867867002`*^9, 3.5250924419948015`*^9, 3.5250925677372727`*^9, 
   3.5250926861781273`*^9, 3.525092890260778*^9, 3.52509320217367*^9, 
   3.5250987621696835`*^9, 3.5254479855037403`*^9, {3.52546119028584*^9, 
   3.525461212941136*^9}, 3.5255182639673758`*^9, 3.525824478843128*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["Dy", 
    RowBox[{"d", ",", "1"}]], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      SubscriptBox["y", 
       RowBox[{"d", ",", "1"}]], "]"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["Dy", 
    RowBox[{"d", ",", "2"}]], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      SubscriptBox["y", 
       RowBox[{"d", ",", "2"}]], "]"}]}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533175133389*^9, 3.513533227600235*^9}, {
   3.513534447833279*^9, 3.513534448428582*^9}, {3.513534581373526*^9, 
   3.51353459872282*^9}, {3.513534853366941*^9, 3.513534855172635*^9}, {
   3.513874463242996*^9, 3.513874465281417*^9}, {3.5158616236643424`*^9, 
   3.5158616650997124`*^9}, {3.5158617380638857`*^9, 3.5158617555558863`*^9}, 
   3.515862210846927*^9, {3.5158724149302444`*^9, 3.5158724662411795`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["DLfy", 
    RowBox[{"d", ",", "1"}]], "=", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      RowBox[{
       SubscriptBox["Dy", 
        RowBox[{"d", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
    "Simplify"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["DLfy", 
    RowBox[{"d", ",", "2"}]], "=", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      RowBox[{
       SubscriptBox["Dy", 
        RowBox[{"d", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
    "Simplify"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158617931210346`*^9, 3.5158618378455925`*^9}, 
   3.5158620205530434`*^9, 3.515862208155773*^9, {3.515872496206894*^9, 
   3.5158725099996824`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate actual kinematics outputs and Jacobians", "Subsubsection",
 CellChangeTimes->{{3.513532922993929*^9, 3.513532938890856*^9}, {
  3.513534921064935*^9, 3.513534931010012*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["y", 
   RowBox[{"a", ",", "1"}]], "=", 
  SubscriptBox["deltav", "hip"]}]], "Input",
 CellChangeTimes->{{3.513532586333204*^9, 3.513532677723161*^9}, {
   3.513532815648106*^9, 3.51353282124899*^9}, {3.513532861207259*^9, 
   3.513532869009141*^9}, {3.513532899901752*^9, 3.513532900419178*^9}, {
   3.51353311611145*^9, 3.513533116605757*^9}, 3.515521088606793*^9, {
   3.5155211643341246`*^9, 3.5155211667002597`*^9}, {3.5158526171702003`*^9, 
   3.515852621396442*^9}, {3.5158548842498703`*^9, 3.5158548875960617`*^9}, {
   3.5158551932005405`*^9, 3.5158551957126846`*^9}, {3.5158593451740203`*^9, 
   3.515859367792314*^9}, 3.51586024539751*^9, {3.5158611465420523`*^9, 
   3.515861146589055*^9}, 3.5158681409047847`*^9, {3.515869758310295*^9, 
   3.5158697588093233`*^9}, {3.515869821362901*^9, 3.515869822510967*^9}, {
   3.5158725172250957`*^9, 3.515872573737328*^9}, 3.5158863481049786`*^9, {
   3.515886571837773*^9, 3.5158865734445753`*^9}, {3.51595325150783*^9, 
   3.5159532575301747`*^9}, {3.5167083309077144`*^9, 
   3.5167083312677355`*^9}, {3.5167110381765614`*^9, 3.5167110528944035`*^9}, 
   3.5167111385673037`*^9, {3.5201093866961327`*^9, 3.520109388523237*^9}, 
   3.5201094274054613`*^9, 3.5201773866844225`*^9, {3.520437239317703*^9, 
   3.5204372535225153`*^9}, {3.5204490557876596`*^9, 
   3.5204490607579436`*^9}, {3.5207159029790354`*^9, 3.520715937276997*^9}}],

Cell[BoxData[
 RowBox[{"-", 
  FractionBox[
   RowBox[{"127", " ", 
    RowBox[{"(", 
     RowBox[{
      RowBox[{"685", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], " ", 
       RowBox[{
        SuperscriptBox[
         SubscriptBox["\[Theta]", "1"], "\[Prime]",
         MultilineFunction->None], "[", "t", "]"}]}], "+", 
      RowBox[{"514", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "1"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "+", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "2"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], 
   "250000"]}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwdx1tIkwEchvGvyEOakW01NkwT8jBh5aiEXOa+peTKjSZGa5XhqS6k3MVq
bDbnRaK4SKtphw0pXWtbgemCSsJq82LGhCgSFuUhSusLixrBrMTevxcPP57M
msaK+hUMw5Qj3u8NDpmMYzdO2F4uuys6XgQLBtkXJbBas50phaZn9c4quFk/
KD0B1eJ5kR1qW3k9ut14+T8XmR+Yc/ihKLm99SH8+pMrzt7DsT5TZWoOlNos
3bnwVdds0kE4bOx4cAwaAgMT5MqTi7oWaN12M3AXvlGmtXhgblv0UGIxx7Z7
vWvFcJ5/Y4a0X/kRdsH7ntfjd6BuRJvlhuvS/b/cco49t08/FoOf46acC3Ax
b0e+keXYDLkgTH5ynZ0zwWqz/pQZ7m/KkDXBe8d5SnK0rfz6eTiUFecmteEZ
P/lufZ8yBicfxZuX/XZLugBjRanNCgXHlk3ndJOGCx5rKSwcCPaRHQrhczJh
SNJwAIZ6Oi+TyQl/lSqYXvuxktRkLtlI9dZCvhrW1PJTDsPvs6NqUiTZEiHX
dIVGtLC5c/VbcuemJxVHYNqSyGCATwUaoQkmuiKne+HjqYjl6l6OTVJ9kNhh
VTSmImVfhhvIS4Z+G/k+5aiDjBeKfeTFaV+QLPCOcWRef+j2NagPe7IVJRzb
aGGcZfBM0Gqsg3WTglV/4H8iviNO
  "]]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{"NSslopeOutput", " ", "=", " ", 
    RowBox[{"{", 
     RowBox[{"nsslope", ",", "LinearNSslope", ",", "HipAngle"}], "}"}]}], 
   ";"}], " "}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"TorsoOutput", " ", "=", " ", 
   RowBox[{"{", 
    RowBox[{
    "TorsoHipAngle", ",", "theta4", ",", "nstorso", ",", "LinearNStorso", ",",
      "storso", ",", "LinearStorso"}], "}"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.5207079879024076`*^9, 3.520708024883049*^9}, {
   3.5207080945102367`*^9, 3.520708221860804*^9}, 3.520708254086914*^9, {
   3.520708319154134*^9, 3.520708321275271*^9}, {3.520709531795629*^9, 
   3.52070953330647*^9}, {3.52070963126945*^9, 3.520709637753376*^9}, {
   3.520709896663495*^9, 3.520709968448744*^9}, {3.520710150638193*^9, 
   3.5207101551185637`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["y", 
   RowBox[{"a", ",", "2"}]], "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"NSslopeOutput", "[", 
         RowBox[{"[", "2", "]"}], "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"TorsoOutput", " ", "[", 
         RowBox[{"[", "1", "]"}], "]"}], "}"}]}], "}"}], "/.", "constsubs"}], 
    "/.", "p0"}], "]"}]}]], "Input",
 CellChangeTimes->{{3.520715958669221*^9, 3.520715959856289*^9}, {
   3.5207177885888863`*^9, 3.520717788818899*^9}, {3.520734705204461*^9, 
   3.52073470640053*^9}, {3.520897217409034*^9, 3.520897218344087*^9}, {
   3.5225102217755737`*^9, 3.5225102219125814`*^9}, {3.522511841977244*^9, 
   3.522511842199257*^9}, {3.525824325681368*^9, 3.5258243265844193`*^9}, 
   3.5269496174676437`*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    FractionBox[
     RowBox[{
      RowBox[{
       RowBox[{"-", "514"}], " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
      RowBox[{"685", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
     RowBox[{
      RowBox[{"514", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{"685", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}]], "}"}],
    ",", 
   RowBox[{"{", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.515868141769834*^9, 3.5158704779924583`*^9, 3.515872613700614*^9, 
   3.5158730124294195`*^9, 3.5158866899455805`*^9, 3.515887483177*^9, 
   3.515889062521902*^9, 3.515890049404812*^9, 3.515890243375561*^9, 
   3.515948288837845*^9, 3.515948936543189*^9, 3.5159533866365595`*^9, 
   3.515953701736582*^9, 3.5159726511817513`*^9, 3.5159857650498214`*^9, 
   3.5167070783170705`*^9, 3.5167097962905293`*^9, 3.5167112854787064`*^9, 
   3.51671262058807*^9, 3.5167319066145887`*^9, 3.5167515599833755`*^9, 
   3.517679227685151*^9, 3.5176796601748877`*^9, 3.517679749097974*^9, 
   3.5176800836771107`*^9, 3.517685993560137*^9, 3.5176980429553227`*^9, 
   3.517698480531351*^9, 3.5176985940348425`*^9, 3.520109552616623*^9, 
   3.5201200218744297`*^9, {3.52017738944958*^9, 3.520177396013956*^9}, 
   3.520177850905974*^9, 3.5201861298185005`*^9, 3.520256997045225*^9, 
   3.5202590887548647`*^9, 3.5203392146814127`*^9, 3.5204373586785297`*^9, 
   3.520437774819332*^9, 3.5204379009545465`*^9, 3.5204436636282454`*^9, 
   3.5204492157118063`*^9, 3.520456964328002*^9, 3.52053285640736*^9, 
   3.5205336409602346`*^9, 3.5207002080262527`*^9, 3.520713024730409*^9, 
   3.5207159927251687`*^9, 3.5207163724048853`*^9, 3.5207204956977243`*^9, 
   3.520734730131887*^9, 3.520735233645686*^9, 3.520787683208625*^9, 
   3.5207888775769386`*^9, 3.520897239459295*^9, 3.5213037286550303`*^9, 
   3.5213038479148517`*^9, 3.521486047291457*^9, 3.521487916293358*^9, 
   3.5222451193780317`*^9, 3.522246794679854*^9, 3.5222498999762444`*^9, 
   3.5223307112511797`*^9, 3.5223566123583145`*^9, 3.522356917539287*^9, 
   3.5224296559307027`*^9, 3.5225103569553056`*^9, 3.5225118729410152`*^9, 
   3.522708143649251*^9, 3.5227751573269463`*^9, 3.522776844828807*^9, 
   3.5230285835666475`*^9, 3.5230306386060915`*^9, 3.5230313388642416`*^9, 
   3.5235572696344223`*^9, 3.5237248755447254`*^9, 3.5237265669924707`*^9, 
   3.52398374567325*^9, 3.5239849927615795`*^9, 3.523989307713381*^9, 
   3.5239903315469403`*^9, 3.523992025014801*^9, 3.5239921366481867`*^9, 
   3.523994818418575*^9, 3.5239949281898537`*^9, 3.523995039079196*^9, 
   3.5242481410828023`*^9, 3.5242485282449465`*^9, 3.5242499970817957`*^9, 
   3.5244001125688143`*^9, 3.5244002972681293`*^9, 3.5244062693301725`*^9, 
   3.524406550146142*^9, 3.524406784821375*^9, 3.524418393457982*^9, 
   3.5244187004931564`*^9, 3.524422223872611*^9, 3.5244223111006002`*^9, 
   3.5244227519828176`*^9, 3.5244238418251534`*^9, 3.524442216654133*^9, 
   3.524442742182192*^9, 3.524443849159507*^9, 3.5244456862975855`*^9, 
   3.524445864326768*^9, 3.524446869825817*^9, 3.5245024328836436`*^9, 
   3.5245119465771503`*^9, 3.524588398829275*^9, 3.5250902868265605`*^9, 
   3.525091694950655*^9, 3.5250919518067093`*^9, 3.5250921898718505`*^9, 
   3.5250924451349792`*^9, 3.5250925708754435`*^9, 3.525092689330307*^9, 
   3.5250928933699107`*^9, 3.5250932063479085`*^9, 3.52509876630392*^9, 
   3.525447989297957*^9, 3.5254612160403132`*^9, 3.525518268027608*^9, 
   3.525824544854904*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  SubscriptBox["Dy", 
   RowBox[{"a", ",", "1"}]], "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    SubscriptBox["\[PartialD]", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
    RowBox[{"Flatten", "[", 
     SubscriptBox["y", 
      RowBox[{"a", ",", "1"}]], "]"}]}], "]"}]}], "\[IndentingNewLine]", 
 RowBox[{
  SubscriptBox["Dy", 
   RowBox[{"a", ",", "2"}]], "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    SubscriptBox["\[PartialD]", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
    RowBox[{"Flatten", "[", 
     SubscriptBox["y", 
      RowBox[{"a", ",", "2"}]], "]"}]}], "]"}]}]}], "Input",
 CellChangeTimes->{{3.513533234344025*^9, 3.5135333201722*^9}, {
   3.513533374269628*^9, 3.513533375964947*^9}, 3.513534856986833*^9, 
   3.513874036445271*^9, {3.513947979870945*^9, 3.513948005409799*^9}, 
   3.515521160512906*^9, 3.5158669774602394`*^9, {3.515872580275702*^9, 
   3.515872610360423*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"-", 
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"-", "685"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], " ", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "1"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}], "-", 
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "1"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}], "+", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "2"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], 
     "250000"]}], ",", 
   FractionBox[
    RowBox[{"32639", " ", 
     RowBox[{"Sin", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
     RowBox[{"(", 
      RowBox[{
       RowBox[{
        SuperscriptBox[
         SubscriptBox["\[Theta]", "1"], "\[Prime]",
         MultilineFunction->None], "[", "t", "]"}], "+", 
       RowBox[{
        SuperscriptBox[
         SubscriptBox["\[Theta]", "2"], "\[Prime]",
         MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "125000"], ",",
    "0", ",", "0", ",", "0", ",", 
   RowBox[{"-", 
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"685", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}], "250000"]}], ",", 
   RowBox[{"-", 
    FractionBox[
     RowBox[{"32639", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], 
     "125000"]}], ",", "0", ",", "0", ",", "0"}], "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwd0mlQzAEYBvAV0jBSo9ptU8vQQZRObIf+KbZpapqUSDJMmBhXs9SuytJh
dKDp1O6k3c7ZTEzFMEiUsCrjap0djuQvUtGxFM/rwzu/eT48n5530c6Dobum
cTicY7jWUcnAbyeWSQ28p5yEy7u21U7BEdPcLjLFe0PWTGeW6as4OEiGX3rd
oA+vDa5aOQuac+JFZGPgktLF8Os7/hAZJdhuaQNV7Xf9yJ9jB9IcqWevHnCD
2sjnCwtcWCZBmy8uhD5NVxyKoN7H8y9IUV6kjwpWc/Vf1UBNXuLjQ24s0y/K
mnkYXgtSG5EbdiQJSSPpGksxLLHd/VcCz1jw3KUwoVjkR94e5iXPF7JMj/Gt
Yh6Mi31X4gpVQdFasqjQ8/1qKD6kStwKG47wT0bB/uZOZQp0sNVm18Mprl0z
qTh94QkZZjiqvAq9R8cirkOr2syFt2Apl+/4Hj6tEuSQLSN6fS4eLPOA5doJ
YZYzT+4BdbpsDZnA17V7wT6B6I4f1Gty4/jDLskeRTSMS6x32g7NHg7y82Cj
iVlBpCfLhLCT5f+Vs/J6uDk2I60Bdph9W2vjzTL8nk3GttCwVpZvB5/P+zI7
BPYfzbocBQ8wdZ2kZGIyUgbrTpXdrYIdrQJZNRRl/go3WMsyo8Iaw6Uw9aK8
l+xe/7OtHP7pftZeATul26wrYYfxleFKH5ZZFhP3cBy6e/UoJqBznfvKeIZl
3rqYt5FDuvjPEpgRHrdHCo3WL/I4Brs/mgaQ952CixJhn1a/kvRa8KGenG5e
FjAOp24YSEkrQbnTBLQ/bJLs64s/ESzNJ0tda477w5iMeyqS62DRRCoDHfcF
wtSeszmkuvRPQBBsffYhjHRV/M0kPWRCk2A4HmI6NwL+ONcaTF4etH5FqkI1
jZuhbM2cF+RqxY3QLTAk3VIshmGXNppLYLXXm/0lsPfT66TcddhnS/eKPLhX
rAsiCx7d3EdqJsszybKXUXKypddeTdpILzb/z7FtLJmerlEWwuE5ahtfP5bh
fZ+mEMHdBSfiY6BnstUMHfwHLZHjMg==
  "]],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{
       RowBox[{"(", 
        RowBox[{"733421", "+", 
         RowBox[{"704180", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], ")"}], 
       "/", 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "2"]}]}], ",", 
     RowBox[{"-", 
      RowBox[{
       RowBox[{"(", 
        RowBox[{"733421", "+", 
         RowBox[{"704180", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], ")"}], 
       "/", 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "2"]}]}], ",", 
     RowBox[{"-", 
      RowBox[{
       RowBox[{"(", 
        RowBox[{"733421", "+", 
         RowBox[{"704180", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], ")"}], 
       "/", 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "2"]}]}], ",", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{"733421", "+", 
        RowBox[{"704180", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], ")"}], 
      "/", 
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"685", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}], "2"]}], ",", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{"685", " ", 
        RowBox[{"(", 
         RowBox[{"685", "+", 
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
         ")"}]}], ")"}], "/", 
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"685", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}], "2"]}], ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
    "0", ",", "1", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0",
      ",", "0", ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
    "0", ",", "0", ",", "0", ",", "0", ",", "1", ",", "0", ",", "0", ",", "0",
      ",", "0", ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
    "1", ",", "1", ",", "1", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0",
      ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwd0mlQzAEYBvAVxTBSo9ptk2XSVqJ00nbon6Vtmpom5UgYJkwMpVlqt8Mi
GR2O0cXupN1SzWZiKoZBIsKqjKt1dZHk70ihYyme14d3fvN8eD497/ytCZHb
JnE4nFRc87Dsy283lskMvaceh4s6N9ZMwB+WpzvJwwHBucbuLNN3PmGAjL74
ut4EXh1YumQqtOYkS8iG0AWldvBzB3+QjBVsthVCTesdMflzZM8RV+o5a794
QX3M83mFHiyToi+QFsHAxssuxdCo98wLUpIfE6iBVVyTV9VQl5/2ONGLZfol
ucZ74dUwrRkZvCVdRJrJfWylsMRh+18ZPG7D85bDlLMSMXlriJcxW8Qy3eY3
z/JgUnxHiSfUhG3Sk8VFfu+WQWmiJm0DrN/HPxQL+5va1Yehi4M+rw5OcB2b
SNWxc0/IKNNh9RUYMDyy9hqcW5Mz7yYs5fJd38GnlYJT5N0fRn0evizzgOU6
imCuO0/pCw2GPB2Zwje0+sM+geS2GBo1enFWwk7ZDtUmmJRW57YZWj0c4OfD
Bgurwhg/lolgx8v/q2SVdXBdfPaRethm9XW5MIBl+N1rzB2gaY2iwBE+n/Vp
egTs3597KRbuYWrbSdnYeIwC1h4tu1MJ25oFiiooyfkVPW05ywyLqk2dYOYF
ZQ/ZtepnSzn80/Ws9Txsl2+0r4Bt5peHKgJZZmFc0sNR6O3frRqD7rXeS5IZ
lnnrYd1CDhqSP8pgdnTSDjk0WzXfNxV29VqGkPfdwovTYJ/epIL0n/O+jpxs
XRYyCieuT5OTcwXlbmPQea9FRlAQ/kTgVECWelYfWAnjsu9pSK6LTSOpDnXd
FQozu0+cIrWlf0LCYPOz91Gkp+pvDumrEFmEw9EIy5lr4feTzeHkpQH7V6Qm
UtewDip8Zrwgl6muR66HEVm2UimMurjaWgar/N/sLoE9H16nn16BfdZ3Lc6H
O6WGMLLw0Y1dpG68PIcsexmrJO/2OGtJofxC0/8c38KSWVk6dREcmqEVBolZ
hvdtkkoCtxceTI6DwhG7KQb4DzBO45o=
  "]]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  SubscriptBox["DLfy", 
   RowBox[{"a", ",", "1"}]], "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    SubscriptBox["\[PartialD]", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
    RowBox[{"Flatten", "[", 
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"a", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], 
   "]"}]}], "\[IndentingNewLine]", 
 RowBox[{
  SubscriptBox["DLfy", 
   RowBox[{"a", ",", "2"}]], "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    SubscriptBox["\[PartialD]", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
    RowBox[{"Flatten", "[", 
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"a", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}]}], "Input",\

 CellChangeTimes->{{3.513533411525342*^9, 3.513533467774484*^9}, {
  3.513534859343821*^9, 3.513534864828146*^9}, {3.513874068222587*^9, 
  3.513874090913892*^9}, {3.513874132914815*^9, 3.513874149954311*^9}, {
  3.513874244874198*^9, 3.513874245301785*^9}, {3.513947988668254*^9, 
  3.513948001956114*^9}, {3.515521142307864*^9, 3.5155211535195055`*^9}, {
  3.515861859950857*^9, 3.515861881451087*^9}, {3.515861991803399*^9, 
  3.5158620021649914`*^9}, {3.5158656112430964`*^9, 3.515865614533284*^9}, {
  3.515872634222788*^9, 3.5158726502537045`*^9}, {3.5201204072844744`*^9, 
  3.52012040879156*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{
     FractionBox["1", "250000"], 
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}], " ", 
         SuperscriptBox[
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "1"], "\[Prime]",
            MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", 
        RowBox[{"1028", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "1"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], " ", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "2"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
         SuperscriptBox[
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "2"], "\[Prime]",
            MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", 
        RowBox[{"685", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], " ", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "1"], "\[Prime]\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "1"], "\[Prime]\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "2"], "\[Prime]\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], ",", 
    RowBox[{
     FractionBox["1", "125000"], 
     RowBox[{"32639", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
         SuperscriptBox[
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "1"], "\[Prime]",
            MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", 
        RowBox[{"2", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "1"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], " ", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "2"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}], "+", 
        RowBox[{
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
         SuperscriptBox[
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "2"], "\[Prime]",
            MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", 
        RowBox[{
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "1"], "\[Prime]\[Prime]",
             MultilineFunction->None], "[", "t", "]"}], "+", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "2"], "\[Prime]\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], 
       ")"}]}]}], ",", "0", ",", "0", ",", "0", ",", 
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}], " ", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "1"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "2"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "125000"], 
    ",", 
    FractionBox[
     RowBox[{"32639", " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["\[Theta]", "1"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "+", 
        RowBox[{
         SuperscriptBox[
          SubscriptBox["\[Theta]", "2"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "62500"], ",",
     "0", ",", "0", ",", "0"}], "}"}], "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwV0H0s1AEcx/Fjtzy0rFaebmqncdgYVxq5wk/OXBeZ2J0jNqnMrbQwO3ma
Hpdu2KGTW3POU1KJY9VS6kyF29REzWqSqf2y26rtOOXq8/3js9d/7z8+vrmF
qScdOBxOJub33H/SK4pl/NQ5c+HQ2R69GAlLxIbyTGiv2lmTBY/cmdNfhNf2
fVAPQlFy21uSx13VD0Mt1yZ7DJcMN/gjkLvBC12E7cX8BlIxzF3eK2KZ2Eqv
wChYluHdKoLffesmyGP5f80H4dXw1Bfx0OgUwRHDrbJ8XTaMPG8U5sDR/T95
jbBP5NmsOIBus72DPPX7R+sgTKivvWyEVh9LjCCaZQo85dsC4BWPmqZAmC1g
XVOg1Kruz4K6WOMs+W6PXVENE5uNL7uho8q3ugcaWqzpzjEsM/35rlsQ3LSi
+0LmZlqnOuCCYsbcCQUD2f5dULg69KsrlmXSCorerMG+6QWdDVqmI8JKGfSC
eFMkz0H1TQVTpEWny+CsaLfoAgzz8ZSQ6oCj2nJ4SefURRoffB0klzUGyRoc
e+hSRo7XdQpt0NHdvTIujmUOXw9qIttc+qrE8HXheDvpFuwzSuYxYUop9LLV
NZCfKjYkSTC8fimNzDL/qyU5+qgdyfDskvsWGaxPf5VM9o8JPpIzoZPP5JBv
3/yeNA08Tc2A54Z2FRfDWUuatwp6rM+fuQ018vkKzSGWWUlYCGmEJfw/SaS2
bURJRj7prCU77h1vJXuEIb3k6Pb7JtJ1wsySj05M6m/CW929grh4/Ghy1CXC
fGVNaR4UTwVz1+F/43ZgKA==
  "]],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{
       RowBox[{"(", 
        RowBox[{"2", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"514", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
           RowBox[{"685", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{"733421", "+", 
              RowBox[{"704180", " ", 
               RowBox[{"Cos", "[", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
             ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "1"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "+", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{"733421", "+", 
              RowBox[{"704180", " ", 
               RowBox[{"Cos", "[", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
             ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "2"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "+", 
           RowBox[{"733421", " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "+", 
           RowBox[{"704180", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "-", 
           RowBox[{"733421", " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "-", 
           RowBox[{"704180", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "-", 
           RowBox[{"469225", " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "-", 
           RowBox[{"352090", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], ")"}], 
       "/", 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "3"]}]}], ",", 
     RowBox[{"-", 
      RowBox[{
       RowBox[{"(", 
        RowBox[{"2", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"514", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
           RowBox[{"685", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{"733421", "+", 
              RowBox[{"704180", " ", 
               RowBox[{"Cos", "[", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
             ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "1"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "+", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{"733421", "+", 
              RowBox[{"704180", " ", 
               RowBox[{"Cos", "[", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
             ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "2"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "+", 
           RowBox[{"733421", " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "+", 
           RowBox[{"704180", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "-", 
           RowBox[{"733421", " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "-", 
           RowBox[{"704180", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "-", 
           RowBox[{"469225", " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "-", 
           RowBox[{"352090", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], ")"}], 
       "/", 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "3"]}]}], ",", 
     RowBox[{"-", 
      RowBox[{
       RowBox[{"(", 
        RowBox[{"2", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"514", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
           RowBox[{"685", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{"733421", "+", 
              RowBox[{"704180", " ", 
               RowBox[{"Cos", "[", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
             ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "1"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "+", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{"733421", "+", 
              RowBox[{"704180", " ", 
               RowBox[{"Cos", "[", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
             ")"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "2"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "+", 
           RowBox[{"733421", " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "+", 
           RowBox[{"704180", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "3"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "-", 
           RowBox[{"733421", " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "-", 
           RowBox[{"704180", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "4"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "-", 
           RowBox[{"469225", " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}], "-", 
           RowBox[{"352090", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], " ", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "5"], "\[Prime]",
              MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], ")"}], 
       "/", 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "3"]}]}], ",", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{"2", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"(", 
            RowBox[{"733421", "+", 
             RowBox[{"704180", " ", 
              RowBox[{"Cos", "[", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
            ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "1"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{"733421", "+", 
             RowBox[{"704180", " ", 
              RowBox[{"Cos", "[", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
            ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "2"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "+", 
          RowBox[{"733421", " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "+", 
          RowBox[{"704180", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "-", 
          RowBox[{"733421", " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "-", 
          RowBox[{"704180", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "-", 
          RowBox[{"469225", " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "-", 
          RowBox[{"352090", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], ")"}], 
      "/", 
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"685", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}], "3"]}], ",", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{"685", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"2", " ", 
           RowBox[{"(", 
            RowBox[{"685", "+", 
             RowBox[{"514", " ", 
              RowBox[{"Cos", "[", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
            ")"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"514", " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], 
             "+", 
             RowBox[{"685", " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
            ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "1"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "+", 
          RowBox[{"2", " ", 
           RowBox[{"(", 
            RowBox[{"685", "+", 
             RowBox[{"514", " ", 
              RowBox[{"Cos", "[", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
            ")"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"514", " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], 
             "+", 
             RowBox[{"685", " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
            ")"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "2"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "+", 
          RowBox[{"1056270", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "+", 
          RowBox[{"352090", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{"2", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}]}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "+", 
          RowBox[{"1202646", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "+", 
          RowBox[{"264196", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "3"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "-", 
          RowBox[{"1056270", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "-", 
          RowBox[{"352090", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{"2", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}]}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "-", 
          RowBox[{"1202646", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "-", 
          RowBox[{"264196", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "4"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "-", 
          RowBox[{"528135", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "-", 
          RowBox[{"176045", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{"2", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}]}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "-", 
          RowBox[{"806352", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}], "-", 
          RowBox[{"132098", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "5"], "\[Prime]",
             MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], ")"}], 
      "/", 
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"685", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}], "3"]}], ",", 
     RowBox[{"-", 
      RowBox[{
       RowBox[{"(", 
        RowBox[{"733421", "+", 
         RowBox[{"704180", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], ")"}], 
       "/", 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "2"]}]}], ",", 
     RowBox[{"-", 
      RowBox[{
       RowBox[{"(", 
        RowBox[{"733421", "+", 
         RowBox[{"704180", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], ")"}], 
       "/", 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "2"]}]}], ",", 
     RowBox[{"-", 
      RowBox[{
       RowBox[{"(", 
        RowBox[{"733421", "+", 
         RowBox[{"704180", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], ")"}], 
       "/", 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "2"]}]}], ",", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{"733421", "+", 
        RowBox[{"704180", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], ")"}], 
      "/", 
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"685", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}], "2"]}], ",", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{"685", " ", 
        RowBox[{"(", 
         RowBox[{"685", "+", 
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
         ")"}]}], ")"}], "/", 
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"685", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}], "2"]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
    "0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "1", ",", "0",
      ",", "0", ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
    "0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0",
      ",", "0", ",", "1"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
    "0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "1", ",", "1", ",", "1",
      ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwV0GtIkwEYxfE5Rl4iKWpehsUsnQ4SXRmaK/U1J5ppsibqXApmJUoZqcjM
G9qNbEzxkuUI57xlVqZTKrIsxUodrDAtpDAbFm8iVDCd5eo8Hw6/b/8Pxzsr
X37SgcPhpGM+z30nPcJYxkeTORsMnezhC6GwSGYoTYf2iu1VKnjkzqy+Gl7d
90EzAKWJrW9JAW9FPwSbebaUx9BiuC4chrx1QeACbCsU1pHKId7iXinLRJZ7
+IfBkjTPFin87q2dII/l/DUdhFeC5S+iodExhCODm1NydBkw9LxRkglH9v8U
NMBeqXuT8gC6TfZ28tTvHy0DMKa25pIRWr2WI0ThLJPrnrrFD152q2r0hxki
1iUJxls1fSqoizTOkO/22JWVMLbJ+LILctXeld3QcNOa7BTBMubPd13FcMOS
7guZlW6daofzymlTBxT1Z/h2QsnK4K/OSJZR5Ba8WYW95nmdDS6bQ4KKGfTE
gilS4KD+poZJ8QWnS+CMdKf0Agzyco8jNX5Hm0vhRZ1jJ2l88HWAXKw3xK3C
sYfOJeS4tkNig1w+vzwqimUOXxM3kq3OvRUy+Dp/vI103e01QmYzQXnx0MOm
rSM/la3HJcDgWouCVJn+1ZAcfdi2RHjWwt+UAmuTXyWSfWOij+R04OSzVCi0
b3xPjvY/lafBc4M7CgvhzLLCUw3d1ubO3Ib1qXNl9YdYZilmPqABFgn/JJDN
rcN5ZOiTjhqy/d7xFrJbEtBDjmy9P0q6TJhY8tGJSf0NeKurRxQVjR9HubpY
mJNXVZwNNbuqeWvwP+MhYB4=
  "]]
}, Open  ]],

Cell[BoxData[""], "Input"]
}, Open  ]],

Cell[CellGroupData[{

Cell["write the control files to disk", "Subsubsection",
 CellChangeTimes->{{3.513533556726437*^9, 3.513533560633779*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<sigma_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"\[Sigma]y", "/.", "\[InvisibleSpace]", "constsubs"}], "/.", 
     "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
   3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
   3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
   3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
   3.513873093721922*^9}, {3.515862148936386*^9, 3.5158621546717143`*^9}, {
   3.516708335792994*^9, 3.516708336128013*^9}, 3.5207161434147873`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<phip_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "hip"], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
   3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
   3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
   3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
   3.513873093721922*^9}, {3.5158698657024374`*^9, 3.5158698742209244`*^9}, {
   3.516708336825053*^9, 3.516708337152072*^9}, 3.520716145044881*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaphip_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["deltap", "hip"], "/.", "\[InvisibleSpace]", "constsubs"}],
      "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5201856101017747`*^9, 3.5201856221114616`*^9}, 
   3.52071614660497*^9, {3.5207164303892016`*^9, 3.5207164345454397`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<phip_dot_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "hipdot"], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
   3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
   3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
   3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
   3.513873093721922*^9}, {3.5158698657024374`*^9, 3.5158698742209244`*^9}, {
   3.516708336825053*^9, 3.516708337152072*^9}, {3.5167541031348357`*^9, 
   3.516754114654495*^9}, 3.520716147794038*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaphip_dot_mat\>\"", "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["deltap", "hipdot"], " ", "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
  3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
  3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
  3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
  3.513873093721922*^9}, {3.5158698657024374`*^9, 3.5158698742209244`*^9}, {
  3.516708336825053*^9, 3.516708337152072*^9}, {3.5167541031348357`*^9, 
  3.516754114654495*^9}, {3.5203390769255333`*^9, 3.520339105876189*^9}, {
  3.520716149641144*^9, 3.5207161499701624`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<pcom_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "COM"], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158901103775215`*^9, 3.5158901154943304`*^9}, {
   3.515948100165904*^9, 3.515948106905116*^9}, {3.5159487206466036`*^9, 
   3.5159487213174047`*^9}, {3.51594878707152*^9, 3.5159487941383324`*^9}, {
   3.515953230430625*^9, 3.51595323192271*^9}, {3.515953761686011*^9, 
   3.515953766147266*^9}, 3.515985484570779*^9, {3.5167083379371166`*^9, 
   3.5167083382401342`*^9}, {3.5207162236953793`*^9, 3.5207162275425997`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<yd1_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158726672456765`*^9, 3.51587268496169*^9}, {
   3.5167083389361734`*^9, 3.516708341929345*^9}, 3.5207162296587205`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<yd2_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, 
   3.513535978245256*^9, {3.5139480491379*^9, 3.513948080396936*^9}, {
   3.513948113545785*^9, 3.513948129942129*^9}, 3.5158619633557715`*^9, {
   3.5158726892259336`*^9, 3.515872692325111*^9}, {3.5167083517449064`*^9, 
   3.5167083521129274`*^9}, 3.5207162311398053`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dyd1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727027727084`*^9, 3.515872707855999*^9}, {
   3.5167083527929664`*^9, 3.5167083530889835`*^9}, 3.5207162325458856`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dyd2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533604687178*^9}, {
   3.513535690806333*^9, 3.513535693339207*^9}, 3.513535980161328*^9, 
   3.51586196000458*^9, {3.515872712110243*^9, 3.5158727149254036`*^9}, {
   3.516708353801024*^9, 3.5167083540880404`*^9}, 3.5207163273033056`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfyd1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.515861942956605*^9, 3.515861955946348*^9}, {
   3.515874032033738*^9, 3.515874045405503*^9}, {3.5167083552411065`*^9, 
   3.5167083555441236`*^9}, 3.520716332000574*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfyd2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.515861942956605*^9, 3.515861955946348*^9}, {
   3.5158740329057875`*^9, 3.515874042230321*^9}, {3.5167083567361917`*^9, 
   3.5167083570722113`*^9}, 3.5207163337346735`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<ya1_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727241079288`*^9, 3.5158727304642925`*^9}, {
   3.516708359905373*^9, 3.5167083605204086`*^9}, 3.52071633560478*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<ya2_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
   3.513535713498742*^9, 3.513535727403304*^9}, {3.5158727332724533`*^9, 
   3.5158727361286163`*^9}, {3.5167083610644393`*^9, 3.516708361376457*^9}, 
   3.5207163401990433`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dya1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727417019353`*^9, 3.51587274563416*^9}, {
   3.5167083619924927`*^9, 3.5167083623125105`*^9}, 3.520716341684128*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dya2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533604687178*^9}, {
   3.513535690806333*^9, 3.51353572494454*^9}, {3.515872742920005*^9, 
   3.5158727481833057`*^9}, {3.5167083632325635`*^9, 3.5167083635285807`*^9}, 
   3.5207163448703103`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfya1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533607692186*^9}, 
   3.513534673091028*^9, {3.513535695836862*^9, 3.513535722564655*^9}, {
   3.513536332459772*^9, 3.513536336316357*^9}, {3.5158727571118164`*^9, 
   3.515872763041156*^9}, {3.516708364688647*^9, 3.516708364976663*^9}, 
   3.520716347555464*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfya2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727601249886`*^9, 3.5158727645362415`*^9}, {
   3.516708366824769*^9, 3.516708367128786*^9}, 3.520716352285734*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["PERL", "Subsection",
 CellChangeTimes->{{3.51353232609383*^9, 3.513532329542585*^9}}],

Cell["this next line assumes that perl is in your path", "Text",
 CellChangeTimes->{{3.513015230408543*^9, 3.513015241200741*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Run", "[", "\"\<perl math2mat_torso.pl\>\"", 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build_torso\>\""}], 
   "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.504476276692404*^9, 3.504476301348216*^9}, {
   3.504478868125564*^9, 3.504478869967337*^9}, {3.504538702237328*^9, 
   3.504538704315194*^9}, {3.504631487786531*^9, 3.504631500259924*^9}, {
   3.504632214564033*^9, 3.504632215194798*^9}, {3.504635416937057*^9, 
   3.50463541912094*^9}, {3.504874639115547*^9, 3.504874639679282*^9}, 
   3.504876166859266*^9, {3.505578303460942*^9, 3.5055783039289427`*^9}, {
   3.512922890945364*^9, 3.512922891900902*^9}, {3.5129244660544987`*^9, 
   3.51292449464812*^9}, {3.512925468730114*^9, 3.512925469241001*^9}, {
   3.513011661780643*^9, 3.513011661926162*^9}, {3.51301497676734*^9, 
   3.513015001300451*^9}, {3.5158700422275343`*^9, 3.515870043613613*^9}, {
   3.515870412159693*^9, 3.5158704163519325`*^9}, {3.5158863672774124`*^9, 
   3.5158863722382207`*^9}}],

Cell[BoxData["0"], "Output",
 CellChangeTimes->{
  3.5155188337118206`*^9, 3.5155213252703295`*^9, {3.515540878198961*^9, 
   3.515540903604343*^9}, 3.5158407423966546`*^9, 3.5158416340670977`*^9, 
   3.5158498815118246`*^9, 3.515850149364145*^9, 3.5158505826739287`*^9, 
   3.5158508174343567`*^9, 3.5158510975593786`*^9, 3.515851653032055*^9, 
   3.5158525885305624`*^9, 3.5158527509368515`*^9, 3.5158527931192636`*^9, 
   3.51585495715904*^9, 3.5158550282321053`*^9, 3.5158552191920276`*^9, 
   3.515855253120968*^9, 3.515865262254135*^9, 3.515865638210639*^9, 
   3.51586592793721*^9, 3.51587048063961*^9, 3.5158730153755884`*^9, 
   3.5158739792467184`*^9, 3.515874203634553*^9, 3.515886694531988*^9, 
   3.5158874913514147`*^9, 3.515889070384316*^9, 3.5158900577352266`*^9, 
   3.515890252205177*^9, 3.5159482968094587`*^9, 3.515948944374403*^9, 
   3.515953395315056*^9, 3.515953710496083*^9, 3.5159537761788397`*^9, 
   3.5159726585821743`*^9, 3.5159857720472217`*^9, 3.516707085463479*^9, 
   3.516709802924909*^9, 3.5167112898219547`*^9, 3.5167126251093287`*^9, 
   3.516731910384804*^9, 3.51675156425562*^9, 3.517679232673436*^9, 
   3.517679752897191*^9, 3.517680087541332*^9, 3.5176859974273577`*^9, 
   3.5176980466975365`*^9, 3.5176984843215675`*^9, 3.517698597905064*^9, 
   3.520109553737687*^9, 3.520120022112444*^9, 3.520177851995036*^9, 
   3.5201861307705555`*^9, 3.520256997752266*^9, 3.5202590894559045`*^9, 
   3.520339215071435*^9, 3.5204373594555745`*^9, 3.5204379012015605`*^9, 
   3.5204436639822655`*^9, 3.520449216190834*^9, 3.520456964627019*^9, 
   3.520532857654432*^9, 3.5205334050077386`*^9, 3.520533641289253*^9, 
   3.520700208631287*^9, 3.520713025182435*^9, 3.520715993050187*^9, 
   3.520716372698902*^9, 3.5207164419848647`*^9, 3.520720498096861*^9, 
   3.520734731372958*^9, 3.5207350240366974`*^9, 3.520735233941703*^9, 
   3.5207876835786457`*^9, 3.520788877843954*^9, 3.5208972402433395`*^9, 
   3.5213037296170855`*^9, 3.5213038482928734`*^9, 3.52148604821651*^9, 
   3.521487916633377*^9, 3.5222451196790495`*^9, 3.522246794946869*^9, 
   3.52224990060228*^9, 3.522330715643431*^9, 3.5223566127951155`*^9, 
   3.5223569178824873`*^9, 3.5224296564487324`*^9, 3.522510361860586*^9, 
   3.5225118733730397`*^9, 3.522708144818318*^9, 3.5227751575219574`*^9, 
   3.522776845738859*^9, 3.5230285840646763`*^9, 3.5230306390121126`*^9, 
   3.523031339818296*^9, 3.5235572703774643`*^9, 3.52372487579274*^9, 
   3.5237265671694813`*^9, 3.5239837459472656`*^9, 3.5239849930135937`*^9, 
   3.5239893079543943`*^9, 3.5239903317719536`*^9, 3.523992025216813*^9, 
   3.5239921368791995`*^9, 3.5239948186215863`*^9, 3.5239949284198666`*^9, 
   3.5239950392682066`*^9, 3.5242481412858143`*^9, 3.524248528452959*^9, 
   3.5242499977068315`*^9, 3.5244001128388147`*^9, 3.5244002976151495`*^9, 
   3.5244062695321836`*^9, 3.524406550370155*^9, 3.5244067850253863`*^9, 
   3.524416267410578*^9, 3.524417771154109*^9, 3.5244178686872463`*^9, 
   3.5244183936479816`*^9, 3.524418700642165*^9, 3.524422224071623*^9, 
   3.5244223112816105`*^9, 3.524422752104824*^9, 3.5244238419211583`*^9, 
   3.524442216849144*^9, 3.524442742387203*^9, 3.5244438493725195`*^9, 
   3.5244456864925966`*^9, 3.5244458645247793`*^9, 3.5244468702188396`*^9, 
   3.5245024335606823`*^9, 3.5245119471381826`*^9, 3.5245883990742893`*^9, 
   3.525090287048571*^9, 3.525091695188668*^9, 3.5250919519797196`*^9, 
   3.5250921900848627`*^9, 3.5250924453299904`*^9, 3.525092571060454*^9, 
   3.52509268956332*^9, 3.5250928935899105`*^9, 3.5250932069259415`*^9, 
   3.525098766927955*^9, 3.5254479897919855`*^9, 3.525461216302328*^9, 
   3.5255182690806684`*^9, 3.525824548871134*^9}]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Zero Dynamics", "Section",
 CellChangeTimes->{{3.513532015025627*^9, 3.513532018772655*^9}, {
  3.5202040661120033`*^9, 3.520204068054115*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<buildopt_torso\>\""}],
    "]"}], ";"}]], "Input"],

Cell[CellGroupData[{

Cell[BoxData[{"statesubs", "\[IndentingNewLine]", 
 RowBox[{"Zstatesubs", " ", "=", " ", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["z", "1"], "[", "t", "]"}], "\[Rule]", " ", 
     FormBox[
      RowBox[{"z", "[", "1", "]"}],
      TraditionalForm]}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["z", "2"], "[", "t", "]"}], "\[Rule]", " ", 
     FormBox[
      RowBox[{"z", "[", "2", "]"}],
      TraditionalForm]}]}], "}"}]}]}], "Input",
 CellChangeTimes->{{3.5202577106190395`*^9, 3.52025774681911*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "1", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "2", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "3", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "4", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "5", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "1"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "6", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "2"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "7", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "3"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "8", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "4"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "9", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "5"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "10", "]"}]}]}], "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwV0VtIkwEcBfBPVyjLZorozBITZ2bpJtIYXqafTZhlYqK1UjTWUtCigoHN
1ZR1AzPIyyzaEBRnS3uYKZHzWhpkOEoqISKrzc3xBbWSUDOw8384/OA8nJez
R32h5GwAwzAqJIAVLZRncCz3SPyJlAvllRVwTSkxkhUJS93XYNu7n3eG4MuD
tnnSpWV6nkLv5yDVCBz+boobh+pjUrEL1jqSW8kds1He9EyOjZxJTMqALqGV
J4fSRUMYGeI591wBp1JzmXx4Waq1VMJgz1haFay9xI/pgC8YWeepLI6dK+Rb
yesevmUI+nSdN4ah37SakyjnWLvydNheaFKbTEnQNli/rRje9ZvtFdA3OrlA
Pl4VlDfBkJGs6YdQJkptssGEqNjjwTkc274xKNgHK32Ob6ThwFZnL2xRuJ1W
WDivF/VBXeyb3325HCtM08+uQaPZa1mHgX9zJPUs9jLi58jQjcZlHYzIbqhp
gL3ipEw9fPB+dwE5tr/s/hV4cVLQR2pWuCEy2d9fsAYNq6EN5M62Z2nrsNQZ
Y8jL49iacpmJHKt60pgPi43OHnJeGT9FrvCkdUeo3+xoJeMkvMNHYVM0V0oG
j/NaSMFcdkQR1Czu2n4CDgw4i0hZYMpHMtL1dkIFB8PDP5DCpVclJ6F7NF2r
hZLY6mgdNN5yn++CXrXravshjrU0L6d0wJuvmSIyRDJTR/7aHLhNVv9Rm0mH
Pb2fbG4enia3THzhyB9fF7vvwSC7IzFPgR//8S1KeKbMWq+B/wGkdFEl
  "]],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["z", "1"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"z", "[", "1", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["z", "2"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"z", "[", "2", "]"}]}]}], "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwV0V1IUwEcBfCrKxzLZorozBomambpNkZjTJ1eU5hlYmK1UjSGKWhRwcCm
NWV9gRmkbhY5BEWXaQ9bSuScH6VBhqNGJURkNbcpNyhLQs3Azv/h8IPzcF7O
Ht2F4rNBDMNokSA2ca5UxbHcI8knUi1Sl5fBNY3URJYl+LqvwbZ3P+8MwZcH
+z2kV8/0PIWBzyHaETj83RI3BnXHFBIvrHGmtJI7ZqID8nSOjZpOSlZBr6iP
p4aKeWM4Geo/9zwXTqZlM3nwskJvLYd8v0tWAWsuCWLN8AWj7DidwbGzBYI+
8rpfYB2CS4aOG8Nw2bKalaTmWLvmTPheaNFZLMmw31G3rQjeXe60l8Gl0Yk5
8vGqsLQJho5kTD2EysS0pn6YEC0+wc/i2PYNh3AfLF9yfiONB7a6e2FL7oK7
DxZ4GhJt0CB+89uWzbEiWcPMGjR1BqzrMPhvlrSOxZ4qfpYM22hcNMDIzPrq
etgrSU5vgA/e784nXfuP378CL04IbWTlCjdEpiwP5K9B42pYPbmz7ZlsHZa4
Y405ORxbXaq0kK6KJ415sMjk7iE9mvhJcoWnqD1C/aa5lYyT8g4fhU0xXAnJ
H+O1kMLZzMhCWDm/a/tJODjoLiSVwakfySjv23EtdEREfCBFvlfFp+DCqFyv
h1JxVYwBmm4tnO+CAZ33avshjrU2L6aa4c3XTCEZKp2uJX9tDt4mq/7oOkmn
XT5ANjcPT5Fbxr9w5I+v8933YIjdmZSTix//Cawa6DPb6irhf6o7UeY=
  "]]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["gamma", "0"], "=", " ", 
   RowBox[{"Simplify", "[", 
    RowBox[{"\[ScriptCapitalD]", "[", 
     RowBox[{"[", 
      RowBox[{"1", ",", ";;"}], "]"}], "]"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["gamma", "f"], " ", "=", " ", 
   RowBox[{"First", "[", 
    RowBox[{"(", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["gamma", "0"], ".", "dq"}], "]"}], ")"}], "]"}]}], 
  ";"}]}], "Input",
 CellChangeTimes->{{3.520204134024888*^9, 3.5202042161295843`*^9}, {
   3.5202044639537587`*^9, 3.520204479306637*^9}, {3.5202074170896683`*^9, 
   3.520207419245792*^9}, 3.520212027467367*^9, 3.5202581458829355`*^9, 
   3.5202581882183566`*^9, 3.5204571668135834`*^9, {3.520532942396279*^9, 
   3.520532967661724*^9}, {3.5205337160885315`*^9, 3.520533716514556*^9}, 
   3.523726665427101*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"Dgamma", "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      SubscriptBox["gamma", "f"], " ", "]"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.520176915486471*^9, 3.5201769568378363`*^9}, {
   3.520189383645609*^9, 3.5201893851646957`*^9}, {3.5202120248492174`*^9, 
   3.5202120250492287`*^9}, 3.520532970318876*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<gamma_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["gamma", "f"], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.520179065374438*^9, 3.5201790709897585`*^9}, 
   3.5250986365254965`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<gamma0_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["gamma", "0"], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input"],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dgamma_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Dgamma", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5201790843905253`*^9, 3.5201790889757876`*^9}, {
  3.520179492078844*^9, 3.520179492829887*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"testgamma", " ", "=", " ", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"(", 
      RowBox[{"D", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"1", "/", "2"}], "*", 
         RowBox[{
          RowBox[{"Transpose", "[", "dq", "]"}], ".", "\[ScriptCapitalD]", 
          ".", "dq"}]}], ",", 
        FormBox[
         RowBox[{"dq", "[", 
          RowBox[{"[", 
           RowBox[{"1", ",", "1"}], "]"}], "]"}],
         TraditionalForm]}], "]"}], ")"}], "[", 
     RowBox[{"[", 
      RowBox[{"1", ",", "1"}], "]"}], "]"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"TrueQ", "[", 
  RowBox[{"testgamma", "\[Equal]", " ", 
   SubscriptBox["gamma", "f"]}], "]"}]}], "Input",
 CellChangeTimes->{{3.5202042657974253`*^9, 3.520204390704569*^9}, {
   3.5202120315446005`*^9, 3.520212040261099*^9}, {3.5202575927382975`*^9, 
   3.5202575929873114`*^9}, 3.5250986431018724`*^9}],

Cell[BoxData["True"], "Output",
 CellChangeTimes->CompressedData["
1:eJwV0GlIkwEcBvApHpWlYRHmwDDBAyGvGrrJ2ru0ec2WlZoODbHb0sp088pm
jQwbTkWxmuTR7DIqlxiY2YsmC6+MGjSS0kB7kSw1maDW8//w8PvwfHoe76zc
pON2PB4vBfn4SDCYLuQY6SXnTCV0DOI05ISdtqUCdi3W6Tph7vb4cdJJ0t7S
BVNDTamvoIc5yPs1VDosB03CH35n9OTtw9LpMBHHnAiZ9hfCd+ZsBzEMzQ90
J0UDEWwU9clrvGjYsxBhyIDjvarQTFhUMsKvgwPfbPVpkRyjEgYYyZxYB0Mn
jPtdozVBtrtC4ivmGIX8rrsfZLd21PvDTf1VGxVQ7lL7XAkLLCYLGXx9t7Ic
JrRW9rfDvILmqw9gH+uRsm4vx6z9LHQLgEPGsUkyfPbiSBtURR8cvQ99rSpf
IzzWMbhglHBM+hvZexusSXhhWIaKocCQQgZ75lyGSU3A2Rk1VAhOniqC9k7u
kcWw7LRLHHmlR9JYAp/cWjGS4f8mTeTmT81xNlhw06mYZFV+Ycuw+6G+XCrl
mKzqqQZSYFvVREPZ1Pc2Uuuofkv+Gtt5Lh66ujbWkkKf9QlyGDz6J5msztmg
I5+WyrYlQrHZyy0F3pB8UJA1h/ZYyZk8a18qXJnwspAvKy8cOQqffdVfzodz
+SK+GjZlL51vgj7z82W1+zjGc/Hvrjqos3c9QC6xX3LILb0DVeSOkc93yGsW
6WPSpGf7yQx+7Cx5b3h/awNMi3H2l0bh/1JPQwwUJa2qsuF/bUBRlw==
  "]]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 SubscriptBox["deltap", "hip"], "\[IndentingNewLine]", 
 SubscriptBox["y", 
  RowBox[{"a", ",", "2"}]]}], "Input",
 CellChangeTimes->{{3.5202061228536425`*^9, 3.520206127161889*^9}, 
   3.5202061836871223`*^9, {3.5202575990176563`*^9, 3.5202575998257027`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"-", "Lc"}], " ", 
   RowBox[{
    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
  RowBox[{"Lt", " ", 
   RowBox[{"(", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], ")"}]}]}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVzmtIkwEYxfGt68hNRbuyD2rEWjrMC2Ho0L1rlpdWwzJtDQVvDdIMU9ym
hC6yEDXKqaXNxOlmmaFtCkFmpUUYGmm1QpMwKXmRboZOmdp5Phx+nG//gIz8
pGwuh8NRY2lri+3ySJbJTF/XRdrigipj4W+L8PUZKI/jpWvgKnfOSEZ4X229
DEvZuho79JxKHCML3bbWPtgj6019DLv6QgL6Yd6yIGQaDhTl3iAH9yt+hEex
zMHxWXEk9Hx/dkM0rDJKfMjlaukLBdRyONxYmJ0VZU6DkhF9WDqcmnQKTXDM
w12vlrJMyUyglcz5s8lsh4VvTBUO6PvyikwUzTL+v8w+e6Hqfne9GGbdbeSr
YHelqUcDBc5eJxkZekBTBiu5VUM2WOBvKe+AoiRhCi+GZbbK9V77YN3h8WnS
0KwbbYOznSfetsMwjUFkhZmS4XmrDD2y+GEXzNhlNy9BtU4SWsywjMbBHyF3
/zw3q4d2oVZrgC3DPtIS+KmYn0A+ucDcLoUVoStW8uPgNwfpe7ItwQU9dJtL
SNe9oPAleF15s0wuZxlT3kwDebF8zRgLPT9Pt5Gl6w3PyTs79+Qlwv7kxlpS
kM8/qoRS1fwp8uvolhpSuSN++zGY5vLzSoGijWMqckEQMUE+zJl8lgpX7H5O
UvuuIPk0DFbcKiqk/0Uq1MOntYvnm+G1B/8u1R5iGe+/C8Em2MfzOk6KWyZy
yUfGV1Wkv8XZRLpjFJ2krGNwiFx1JMyRA+4jlgYYuI0nlitYpvqD0BwHvzes
6bLgfwYEWbQ=
  "]],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
     FractionBox[
      RowBox[{"685", " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "1199"]}], "}"}], 
   ",", 
   RowBox[{"{", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVzmtIkwEYxfGt68hNRbuyD2rEWjrMC2HMoXvXrKmthmWaDQVvDdIMU9ym
hC6yEDVqU8uaidNpmaFtCkFmpUUYGmm1QpMwIXmRboZOmdp5Phx+nG//oMyC
pBwuh8NJw9LXFtsUUpbJyljXRbarQqri4G+b8M0ZqFDxMrRwlTtnIqN8r7Zc
hmVsXa0Dek8ljpFFnvaWPtgj7019Arv6woL6Yf6yIGwaDhTn3SAH9yt/REaz
zMHxWbEUen84uyEGVpskfuRyjeylEuo4HG4czMmOtqZDyYghIgNOTbqEFjjm
5alPk7FM6Uywncz9s8nqgEVvLZVO6P/qilwUwzKBv6x+e6HmQXe9GGbfa+Rr
YHeVpUcLBa5eFykNP6Ath1Xc6qF2WBhoq+iAoiRhCi+WZbYqDD77YN3h8WnS
2KQfbYWznSfetcEIrVFkh1mS4Xm7HD3y+GE3zNzlsC7BNL0kvIRhGa2TP0Lu
/nlu1gAdQp3OCJuH/WSl8HMJP4F8eoG5XQYrw1fs5KfB707S/2Rrght66TeX
ku77IZFL8Lr6ZrlCwTKW/JkG8mLFmikOen+ZbiXL1htfkHd37slPhP3JjWZS
UMA/qoYyzfwp8tvollpSvSN++zGY7g7wSYGijWMackEQNUE+yp18ngpXHAEu
Uve+MPk0DFXeKi6i/1UmNMBn5sXzTfDaw3+XzIdYxvfvQqgF9vF8jpPi5ok8
8rHpdTUZaHPdIT2xyk5S3jE4RK46E+bIAc8RWwMM3sYTK5QsU/NRaFVBVcea
Phv+BwGLWSc=
  "]]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"c", " ", "=", " ", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       SubscriptBox["deltap", "hip"], "]"}]}], "/.", "\[InvisibleSpace]", 
     "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"H", " ", "=", " ", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       SubscriptBox["y", 
        RowBox[{"a", ",", "2"}]], "]"}]}], "/.", "\[InvisibleSpace]", 
     "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{"Phi", " ", "=", " ", 
  RowBox[{"Join", "[", 
   RowBox[{
    RowBox[{"{", "c", "}"}], ",", "H"}], "]"}]}]}], "Input",
 CellChangeTimes->{{3.520206779207184*^9, 3.5202069001341004`*^9}, {
  3.520720978686349*^9, 3.5207209931991796`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      FractionBox["152273", "250000"]}], ",", 
     RowBox[{"-", 
      FractionBox["32639", "125000"]}], ",", "0", ",", "0", ",", "0"}], "}"}],
    ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "1"}], ",", 
     RowBox[{"-", "1"}], ",", 
     RowBox[{"-", "1"}], ",", "1", ",", 
     FractionBox["685", "1199"]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"0", ",", "1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"1", ",", "1", ",", "1", ",", "0", ",", "0"}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.5202067842794743`*^9, {3.5202068164733152`*^9, 3.520206900615128*^9}, 
   3.5202112975416174`*^9, 3.520212052913822*^9, 3.5202576025528584`*^9, 
   3.5202588082888227`*^9, 3.5202590954312463`*^9, 3.5203392202527313`*^9, 
   3.520437364716875*^9, 3.5204379063928576`*^9, 3.520443669277569*^9, 
   3.5204492217051497`*^9, 3.5204569695523005`*^9, 3.5205328646758337`*^9, 
   3.5205336434153748`*^9, 3.520700218158832*^9, 3.520713031300785*^9, 
   3.5207209313126397`*^9, {3.5207209819795375`*^9, 3.5207209939802237`*^9}, 
   3.5207210414189377`*^9, 3.520734737932333*^9, 3.5207352400280514`*^9, 
   3.5207876898810067`*^9, 3.520788883833297*^9, 3.5208972468487177`*^9, 
   3.5213037359084454`*^9, 3.5213038573693924`*^9, 3.5214860562599697`*^9, 
   3.5214879246808376`*^9, 3.5222451242133083`*^9, 3.5222468049684424`*^9, 
   3.522249909583794*^9, 3.522330724952964*^9, 3.5223566206887293`*^9, 
   3.5223569257293015`*^9, 3.5224296656732597`*^9, 3.522510372632202*^9, 
   3.522511885147713*^9, 3.52270815289278*^9, 3.5227751638393188`*^9, 
   3.5227768546433687`*^9, 3.5230285958413496`*^9, 3.523030650660737*^9, 
   3.523031347846755*^9, 3.5235572783489203`*^9, 3.523724882308112*^9, 
   3.52372657362185*^9, 3.5239837534076924`*^9, 3.5239850004210176`*^9, 
   3.5239893153998203`*^9, 3.523990339280383*^9, 3.523992032614236*^9, 
   3.5239921444226313`*^9, 3.523994826073013*^9, 3.523994935933296*^9, 
   3.5239950471426573`*^9, 3.5242481491792655`*^9, 3.5242485362624054`*^9, 
   3.5242500094085007`*^9, 3.5244001325719676`*^9, 3.524400319163378*^9, 
   3.5244062960185537`*^9, 3.524406567117997*^9, 3.524406795840927*^9, 
   3.524418409158004*^9, 3.524418716842966*^9, 3.5244222404295588`*^9, 
   3.5244223276375465`*^9, 3.5244227683437533`*^9, 3.524423858332097*^9, 
   3.5244422330230694`*^9, 3.5244427585841293`*^9, 3.5244438657474556`*^9, 
   3.5244457029135356`*^9, 3.5244458810147223`*^9, 3.5244468916260643`*^9, 
   3.524502444798325*^9, 3.5245119619350286`*^9, 3.524588407760786*^9, 
   3.525090295687063*^9, 3.525091703746051*^9, 3.5250919605008154`*^9, 
   3.5250921988863015`*^9, 3.5250924542204547`*^9, 3.5250925747666645`*^9, 
   3.525092698174813*^9, 3.5250929021442895`*^9, 3.5250932183815966`*^9, 
   3.525098778291605*^9, 3.5254480002765846`*^9, 3.5254612248908195`*^9, 
   3.5255182800162935`*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"fbz", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
       FractionBox[
        RowBox[{
         RowBox[{
          SubscriptBox["z", "1"], "[", "t", "]"}], "-", 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "2"}], "]"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{"fbz", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
      FractionBox[
       RowBox[{
        RowBox[{
         SubscriptBox["z", "1"], "[", "t", "]"}], "-", 
        RowBox[{"p", "[", "1", "]"}]}], 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";"}]}]], "Input",
 CellChangeTimes->{{3.5202070301795387`*^9, 3.520207046450469*^9}, {
  3.5207210011976366`*^9, 3.5207210068069577`*^9}, {3.5223568926491346`*^9, 
  3.5223568975475435`*^9}, {3.5224294664548655`*^9, 3.5224294698470592`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["yz", 
    RowBox[{"d", ",", "2"}]], " ", "=", " ", 
   RowBox[{
    RowBox[{"Transpose", "[", 
     RowBox[{"{", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", " ", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
     "]"}], "/.", "fbz"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5202070485075865`*^9, 3.5202070659115825`*^9}, 
   3.523726675705689*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"PhiI", "=", " ", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"Inverse", "[", "Phi", "]"}], ".", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["z", "1"], "[", "t", "]"}], "}"}], "}"}], ",", 
       SubscriptBox["yz", 
        RowBox[{"d", ",", "2"}]]}], " ", "]"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5202071326113973`*^9, 3.520207202280382*^9}, {
   3.5202072693732195`*^9, 3.5202072797958155`*^9}, {3.520207350020832*^9, 
   3.5202073515609207`*^9}, {3.5202111360613813`*^9, 3.520211136449404*^9}, 
   3.520211291781288*^9, 3.5237266786178555`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"PhidotI", " ", "=", " ", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"Inverse", "[", "Phi", "]"}], ".", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["z", "2"], "[", "t", "]"}], "}"}], "}"}], ",", 
       RowBox[{
        RowBox[{"D", "[", 
         RowBox[{
          SubscriptBox["yz", 
           RowBox[{"d", ",", "2"}]], ",", 
          RowBox[{
           SubscriptBox["z", "1"], "[", "t", "]"}]}], "]"}], "*", 
        RowBox[{
         SubscriptBox["z", "2"], "[", "t", "]"}]}]}], "]"}]}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.520272225237257*^9, 3.520272265975587*^9}, {
   3.5202741201076374`*^9, 3.520274121268704*^9}, 3.5237266819780474`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"zcond", " ", "=", " ", 
   RowBox[{"Table", "[", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
      RowBox[{"PhiI", "[", 
       RowBox[{"[", 
        RowBox[{"i", ",", "1"}], "]"}], "]"}]}], ",", 
     RowBox[{"{", 
      RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], ";"}]], "Input",\

 CellChangeTimes->{{3.5202579006819105`*^9, 3.5202579420752783`*^9}}],

Cell[CellGroupData[{

Cell[BoxData["zcond"], "Input",
 CellChangeTimes->{{3.5207002643654747`*^9, 3.520700264954509*^9}, 
   3.523726689928502*^9, 3.5239981481580253`*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{
     FractionBox["1", "152273"], 
     RowBox[{"2", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"-", "32639"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"3", ",", "5"}], "]"}], "+", 
           RowBox[{
            SuperscriptBox["\[ExponentialE]", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "4"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"p", "[", "1", "]"}], "-", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]]], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "1"}], "]"}], " ", 
               RowBox[{"Cos", "[", 
                FractionBox[
                 RowBox[{
                  RowBox[{"a", "[", 
                   RowBox[{"3", ",", "2"}], "]"}], " ", 
                  RowBox[{"(", 
                   RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                 RowBox[{"a", "[", 
                  RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "3"}], "]"}], " ", 
               RowBox[{"Sin", "[", 
                FractionBox[
                 RowBox[{
                  RowBox[{"a", "[", 
                   RowBox[{"3", ",", "2"}], "]"}], " ", 
                  RowBox[{"(", 
                   RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                 RowBox[{"a", "[", 
                  RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], 
          ")"}]}], "-", 
        RowBox[{"125000", " ", 
         RowBox[{
          SubscriptBox["z", "1"], "[", "t", "]"}]}]}], ")"}]}]}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"3", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"5", ",", "5"}], "]"}], "-", 
     RowBox[{
      FractionBox["1", "1199"], 
      RowBox[{"685", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "5"}], "]"}], "+", 
         RowBox[{
          SuperscriptBox["\[ExponentialE]", 
           FractionBox[
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "4"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{"p", "[", "1", "]"}], "-", 
               RowBox[{
                SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]]], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "1"}], "]"}], " ", 
             RowBox[{"Cos", "[", 
              FractionBox[
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"3", ",", "2"}], "]"}], " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"p", "[", "1", "]"}]}], "+", 
                  RowBox[{
                   SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "3"}], "]"}], " ", 
             RowBox[{"Sin", "[", 
              FractionBox[
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"3", ",", "2"}], "]"}], " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"p", "[", "1", "]"}]}], "+", 
                  RowBox[{
                   SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], 
        ")"}]}]}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "+", 
     FractionBox[
      RowBox[{"250000", " ", 
       RowBox[{
        SubscriptBox["z", "1"], "[", "t", "]"}]}], "152273"]}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"2", ",", "5"}], "]"}], "+", 
     RowBox[{"a", "[", 
      RowBox[{"5", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "-", 
     RowBox[{
      FractionBox["1", "1199"], 
      RowBox[{"685", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "5"}], "]"}], "+", 
         RowBox[{
          SuperscriptBox["\[ExponentialE]", 
           FractionBox[
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "4"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{"p", "[", "1", "]"}], "-", 
               RowBox[{
                SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]]], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "1"}], "]"}], " ", 
             RowBox[{"Cos", "[", 
              FractionBox[
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"4", ",", "2"}], "]"}], " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"p", "[", "1", "]"}]}], "+", 
                  RowBox[{
                   SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "3"}], "]"}], " ", 
             RowBox[{"Sin", "[", 
              FractionBox[
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"4", ",", "2"}], "]"}], " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"p", "[", "1", "]"}]}], "+", 
                  RowBox[{
                   SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], 
        ")"}]}]}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"4", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}]}]}], 
  "}"}]], "Output",
 CellChangeTimes->{3.5239981487250576`*^9, 3.5242481550216*^9, 
  3.524248542093739*^9, 3.5242500186200275`*^9, 3.5244001432544813`*^9, 
  3.524400324890705*^9, 3.524406301797882*^9, 3.524406572802245*^9, 
  3.524406801587221*^9, 3.5244184146180115`*^9, 3.524418722507187*^9, 
  3.524422246184888*^9, 3.524422333409877*^9, 3.524422774104083*^9, 
  3.5244238641564302`*^9, 3.524442238684393*^9, 3.5244427643394585`*^9, 
  3.524443871514786*^9, 3.524445708687866*^9, 3.5244458868160543`*^9, 
  3.5244468993685074`*^9, 3.524502452687776*^9, 3.524511971203559*^9, 
  3.524588412981085*^9, 3.5250903009013567`*^9, 3.5250917088682775`*^9, 
  3.5250919656850257`*^9, 3.525092204118601*^9, 3.525092459469742*^9, 
  3.525092580045958*^9, 3.525092703432102*^9, 3.5250929073974905`*^9, 
  3.5250932254049983`*^9, 3.5250987852260017`*^9, 3.5254480068749623`*^9, 
  3.5254612301661215`*^9, 3.5255182870776978`*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Phi_inverse_mat\>\"", "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"PhiI", "/.", "Zstatesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5202568164348955`*^9, 3.5202568378191185`*^9}, 
   3.5202577668162537`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Phi_dot_inverse_mat\>\"", "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"PhidotI", "/.", "Zstatesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5202723515834837`*^9, 3.5202723547236633`*^9}}],

Cell[BoxData[
 RowBox[{
  SubscriptBox["gamma", "0"], ";"}]], "Input",
 CellChangeTimes->{3.523726693225691*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"Psi", " ", "=", " ", 
   RowBox[{
    RowBox[{"Simplify", "[", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"{", 
        SubscriptBox["gamma", "0"], "}"}], ",", 
       RowBox[{"H", "-", 
        RowBox[{
         RowBox[{"D", "[", 
          RowBox[{
           SubscriptBox["yz", 
            RowBox[{"d", ",", "2"}]], ",", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], "]"}], ".", 
         RowBox[{"{", "c", "}"}]}]}]}], "]"}], "]"}], "/.", "zcond"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.520209256926901*^9, 3.5202092572579203`*^9}, {
   3.5202114068938723`*^9, 3.520211412850213*^9}, {3.520257956435099*^9, 
   3.520257981706545*^9}, {3.52025812288262*^9, 3.5202581320631447`*^9}, 
   3.520457151493707*^9, 3.5237267013301544`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Psi_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Psi", "/.", "Zstatesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5202582707790785`*^9, 3.5202582747863083`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["kappa", "1"], "=", 
  RowBox[{"First", "[", 
   RowBox[{"c", ".", "dq"}], "]"}]}]], "Input",
 CellChangeTimes->{{3.520211459547884*^9, 3.5202114829702234`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"-", 
   FractionBox[
    RowBox[{"152273", " ", 
     RowBox[{
      SuperscriptBox[
       SubscriptBox["\[Theta]", "1"], "\[Prime]",
       MultilineFunction->None], "[", "t", "]"}]}], "250000"]}], "-", 
  FractionBox[
   RowBox[{"32639", " ", 
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "2"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}]}], "125000"]}]], "Output",
 CellChangeTimes->{{3.520211465411219*^9, 3.520211483339245*^9}, 
   3.5202120712548714`*^9, {3.520258437445612*^9, 3.5202584486022496`*^9}, 
   3.5202588109279737`*^9, 3.520259098870443*^9, 3.5203392286322107`*^9, 
   3.5204373732643642`*^9, 3.520437916144415*^9, 3.520443681244253*^9, 
   3.520449229828614*^9, 3.520456978048787*^9, 3.5205328732613244`*^9, 
   3.5205336522438793`*^9, 3.5207002268173275`*^9, 3.5207130416243753`*^9, 
   3.520720945618458*^9, 3.5207210501534367`*^9, 3.520734746329814*^9, 
   3.520735248081512*^9, 3.520787697964469*^9, 3.520788891725748*^9, 
   3.520897256579274*^9, 3.5213037474071026`*^9, 3.5213038670269446`*^9, 
   3.5214860654524956`*^9, 3.521487933327332*^9, 3.5222451317327385`*^9, 
   3.5222468158310633`*^9, 3.522249920045392*^9, 3.522330735753581*^9, 
   3.5223566291595445`*^9, 3.522356933966116*^9, 3.522429675128801*^9, 
   3.5225104823554783`*^9, 3.522511897395414*^9, 3.522708162410324*^9, 
   3.522775171237742*^9, 3.522776865018962*^9, 3.523028607492016*^9, 
   3.523031357129286*^9, 3.5235572879864717`*^9, 3.523724889646532*^9, 
   3.5237265809732704`*^9, 3.5239837607151103`*^9, 3.52398500780044*^9, 
   3.5239893227652416`*^9, 3.523990346785812*^9, 3.5239920400616617`*^9, 
   3.5239921519980645`*^9, 3.52399483354344*^9, 3.5239949434607267`*^9, 
   3.5239950546120844`*^9, 3.524248156566688*^9, 3.5242485436088257`*^9, 
   3.5242500210791683`*^9, 3.5244001538240595`*^9, 3.524400326753812*^9, 
   3.524406303560954*^9, 3.5244065743343325`*^9, 3.5244068035403023`*^9, 
   3.5244184165880146`*^9, 3.524418724587306*^9, 3.524422248267007*^9, 
   3.524422335479995*^9, 3.5244227762682066`*^9, 3.5244238662185483`*^9, 
   3.5244422407225094`*^9, 3.524442766389576*^9, 3.524443873578904*^9, 
   3.524445710757984*^9, 3.524445888888173*^9, 3.524446901952655*^9, 
   3.524502454778896*^9, 3.524511973936715*^9, 3.524588414683182*^9, 
   3.525090302566452*^9, 3.5250917104503574`*^9, 3.5250919672601113`*^9, 
   3.525092205832699*^9, 3.5250924612118397`*^9, 3.52509258130803*^9, 
   3.5250927051111956`*^9, 3.5250929090735626`*^9, 3.5250932276441264`*^9, 
   3.525098787399126*^9, 3.52544800892408*^9, 3.5254612318152156`*^9, 
   3.5255182893608284`*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["kappa", "2"], "=", 
   RowBox[{"-", 
    RowBox[{"Simplify", "[", 
     RowBox[{"D", "[", 
      RowBox[{"V", ",", 
       FormBox[
        RowBox[{"q", "[", 
         RowBox[{"[", 
          RowBox[{"1", ",", "1"}], "]"}], "]"}],
        TraditionalForm]}], "]"}], "]"}]}]}], ";"}]], "Input",
 CellChangeTimes->{{3.52020493620277*^9, 3.5202049768280935`*^9}, {
   3.5202113235651064`*^9, 3.520211336885868*^9}, 3.52027004142235*^9, 
   3.523726707345499*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["f", "reduced"], " ", "=", " ", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{"{", 
     SubscriptBox["kappa", "1"], "}"}], ",", 
    RowBox[{"{", 
     SubscriptBox["kappa", "2"], "}"}]}], "}"}]}]], "Input",
 CellChangeTimes->{{3.5202584183535194`*^9, 3.520258464426155*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      FractionBox[
       RowBox[{"152273", " ", 
        RowBox[{
         SuperscriptBox[
          SubscriptBox["\[Theta]", "1"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}]}], "250000"]}], "-", 
     FractionBox[
      RowBox[{"32639", " ", 
       RowBox[{
        SuperscriptBox[
         SubscriptBox["\[Theta]", "2"], "\[Prime]",
         MultilineFunction->None], "[", "t", "]"}]}], "125000"]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"-", 
     RowBox[{
      FractionBox["1", "5000000000000"], 
      RowBox[{"981", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"606150", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
         RowBox[{"20217960", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"606150", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
         RowBox[{"57145291195", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "-", 
         RowBox[{"36585181337", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
         RowBox[{"2781695755", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"6820902539", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"701338095", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}]}]}]}], "}"}]}], "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwdy2lI0wEcxvHZC11mInRYY4qJZdph6jRzXn/PvFYWzqPlsJbRzCtN8ahZ
g9JAFB06RYbVdENEKk0rGJi2BdoolpUjzQwr+4Mg3qsX9fx68fB583z3XSg6
c8mOw+FkYZIHjsckoSzzMShfQIqdtiaSrabLUnLyjkxJJhmF07lQzut8qIRR
gkeNA9B0r/k92fciVDsEP5tHs57DpZFETwMsdgv0/waHXJuaydvruQuBQpY5
0pLnGwpTRsLsI2CZV8oOsl2RNhYLl923bYmDXeMJmhy4sHEzUAq5qzw3FXXz
19XZYSyzO0+uIwcLQzQDsEs0cHcQtrbXMAciWEZ7w2GnN5wtcFIfhEcv2juf
hry2/icSaPjxeooMLs0/XwvnLWctOrhiOKHUwzpzeiY3kmWmPAtdfGAt33ee
1Mv+vNVClaTPuwf6zXHXeqJYxq5EOLEJuevdGhu8puIHVDAsIyz+ayb1xTm/
KuGIv/RKFfR94xBeDffssk8m4xOOd9RAfv2qjpSXWJ+Sn6Tq5E3Yo7dVk89M
kwIbnJiNVEZHs0zp7xk16flzQxkHHYffdZNiUcXof+v3FibDCI1aRWbnOKem
wrrItQwyaXF7Izk2l+IqglFWvksGVFdb0siO3JBp0l3BvsyEvV88psgm6Stx
FlyJnysvg8EN4/xK+D1kpkgD03lWRUsMfsYPfirodW7pFBl+dbSA9BjubyAz
ex93kstfD/WR97N7jWTAYcUiaV0u17bB8Vv7faJjWUYWw9GchNzgoCoZ/Aen
IFYy
  "]]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<f_reduced_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["f", "reduced"], " ", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5202585922104635`*^9, 3.520258635418935*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{"z1", " ", "=", " ", 
  RowBox[{"First", "[", 
   RowBox[{"c", ".", "q"}], "]"}]}], "\[IndentingNewLine]", 
 RowBox[{"z2", " ", "=", " ", 
  SubscriptBox["gamma", "f"]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"zcomp", " ", "=", " ", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"{", "z1", "}"}], ",", 
     RowBox[{"{", "z2", "}"}]}], "}"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.5202118510802784`*^9, 3.5202118657551174`*^9}, {
  3.5202554384430785`*^9, 3.5202554402821836`*^9}, {3.52025849616197*^9, 
  3.520258513369954*^9}, {3.520258666034686*^9, 3.5202586686258345`*^9}, {
  3.520533697077444*^9, 3.520533697543471*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"-", 
   FractionBox[
    RowBox[{"152273", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "250000"]}], "-", 
  FractionBox[
   RowBox[{"32639", " ", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "125000"]}]], "Output",\

 CellChangeTimes->{
  3.520211866067136*^9, 3.520212076437168*^9, {3.5202585043744397`*^9, 
   3.52025851417*^9}, 3.520258669168865*^9, 3.5202588112999945`*^9, 
   3.520259099187461*^9, 3.5203392289192266`*^9, 3.52043737354138*^9, 
   3.5204379168174534`*^9, 3.5204436819222918`*^9, 3.520449230129631*^9, 
   3.5204569783388033`*^9, 3.520532873668348*^9, 3.5205336525428967`*^9, {
   3.520533698827544*^9, 3.520533727760199*^9}, 3.520700227091343*^9, 
   3.520713043701494*^9, 3.5207209459524775`*^9, 3.5207210504084516`*^9, 
   3.5207347465638266`*^9, 3.52073524838853*^9, 3.5207876982934875`*^9, 
   3.5207888920017643`*^9, 3.5208972568862915`*^9, 3.5213037477381215`*^9, 
   3.5213038673559637`*^9, 3.5214860658185167`*^9, 3.5214879336503506`*^9, 
   3.5222451319227495`*^9, 3.522246816380095*^9, 3.5222499204934177`*^9, 
   3.5223307363826175`*^9, 3.5223566295027447`*^9, 3.5223569342937164`*^9, 
   3.5224296753688145`*^9, 3.5225104826604958`*^9, 3.5225118978684406`*^9, 
   3.5227081627193418`*^9, 3.5227751714727554`*^9, 3.5227768653569813`*^9, 
   3.5230286079400415`*^9, 3.5230313574223027`*^9, 3.5235572883124905`*^9, 
   3.523724889884546*^9, 3.5237265812232847`*^9, 3.523983760917122*^9, 
   3.523985008005451*^9, 3.523989322982254*^9, 3.5239903470068245`*^9, 
   3.5239920402676735`*^9, 3.523992152226077*^9, 3.523994833751452*^9, 
   3.5239949436657386`*^9, 3.523995054819096*^9, 3.5242481567987013`*^9, 
   3.5242485438358383`*^9, 3.5242500227322626`*^9, 3.5244001540110693`*^9, 
   3.5244003269608235`*^9, 3.524406303778967*^9, 3.5244065745553455`*^9, 
   3.524406803762315*^9, 3.5244184168180146`*^9, 3.5244187247993183`*^9, 
   3.5244222484800186`*^9, 3.5244223356950073`*^9, 3.524422776451217*^9, 
   3.5244238664325604`*^9, 3.524442240925521*^9, 3.5244427666135893`*^9, 
   3.5244438737929163`*^9, 3.5244457113690195`*^9, 3.5244458890501823`*^9, 
   3.524446902178668*^9, 3.524502455081913*^9, 3.52451197436874*^9, 
   3.524588414954198*^9, 3.525090302809466*^9, 3.5250917107493706`*^9, 
   3.525091967552128*^9, 3.5250922061067147`*^9, 3.5250924614848557`*^9, 
   3.525092581585046*^9, 3.525092705418212*^9, 3.5250929093635635`*^9, 
   3.525093228033149*^9, 3.5250987877541466`*^9, 3.5254480091990952`*^9, 
   3.525461232036228*^9, 3.5255182897058477`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{
    RowBox[{
     RowBox[{"(", 
      RowBox[{"3870080597176665578268687175", "+", 
       RowBox[{"6849201003061543875", " ", 
        SuperscriptBox[
         RowBox[{"(", 
          RowBox[{"12815", "+", 
           RowBox[{"34798", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}]}], 
          ")"}], "2"]}], "+", 
       RowBox[{"18152087754923059077000", " ", 
        SuperscriptBox[
         RowBox[{"(", 
          RowBox[{"213", "+", 
           RowBox[{"1028", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "]"}]}], "+", 
           RowBox[{"1370", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}], "2"]}], "+", 
       RowBox[{"363767545544437671", " ", 
        SuperscriptBox[
         RowBox[{"(", 
          RowBox[{"4985", "+", 
           RowBox[{"130556", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "]"}]}], "+", 
           RowBox[{"173990", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}], "2"]}], "+", 
       RowBox[{"273968040122461755", " ", 
        SuperscriptBox[
         RowBox[{"(", 
          RowBox[{
           RowBox[{"-", "66481"}], "+", 
           RowBox[{"130556", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
           RowBox[{"173990", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}], "2"]}], "+", 
       RowBox[{"96628932273828423", " ", 
        SuperscriptBox[
         RowBox[{"(", 
          RowBox[{"32805", "-", 
           RowBox[{"130556", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "-", 
           RowBox[{"173990", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
           RowBox[{"130556", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
          ")"}], "2"]}], "+", 
       RowBox[{"27396804012246175500", " ", 
        SuperscriptBox[
         RowBox[{"(", 
          RowBox[{"1", "+", 
           RowBox[{"17399", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}]}], 
          ")"}], "2"]}], "+", 
       RowBox[{"3046041890881974684", " ", 
        SuperscriptBox[
         RowBox[{"(", 
          RowBox[{
           RowBox[{"-", "60"}], "+", 
           RowBox[{"65278", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "]"}]}], "+", 
           RowBox[{"86995", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}], "2"]}], "+", 
       RowBox[{"1095872160489847020", " ", 
        SuperscriptBox[
         RowBox[{"(", 
          RowBox[{"5", "+", 
           RowBox[{"65278", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
           RowBox[{"86995", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}], "2"]}], "+", 
       RowBox[{"6234112194578314538268", " ", 
        SuperscriptBox[
         RowBox[{"(", 
          RowBox[{
           RowBox[{"514", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
           RowBox[{"685", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"Sin", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
          ")"}], "2"]}]}], ")"}], " ", 
     RowBox[{
      SuperscriptBox[
       SubscriptBox["\[Theta]", "1"], "\[Prime]",
       MultilineFunction->None], "[", "t", "]"}]}], "+", 
    RowBox[{"2", " ", 
     RowBox[{"(", 
      RowBox[{"16070778793235444729950585186", "+", 
       RowBox[{"18517754738188387040402264655", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "+", 
       RowBox[{"4211401541014329236542981860", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "]"}]}], "+", 
       RowBox[{"3100839940628289377755188000", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{"2", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"2806235462640871135245080325", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"4132442333327584092922770000", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{"2", " ", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]}], "]"}]}], "-", 
       
       RowBox[{"4024931311275503826435792708", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"2681982439906342530261204285", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"413851047162105459574532340", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"275766505161519688529722425", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"413851047162105459574532340", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}], "+", 
       RowBox[{"238338496504535603762250", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "-", 
       RowBox[{"11930371353179612605329120", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "]"}]}], "-", 
       RowBox[{"7949712428918321629037400", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"357681714462281168857800", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"238338496504535603762250", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}]}], ")"}],
      " ", 
     RowBox[{
      SuperscriptBox[
       SubscriptBox["\[Theta]", "2"], "\[Prime]",
       MultilineFunction->None], "[", "t", "]"}]}], "+", 
    RowBox[{
     RowBox[{"(", 
      RowBox[{"4795731522963619078565862533", "+", 
       RowBox[{"4211401541014329236542981860", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "]"}]}], "+", 
       RowBox[{"5612470925281742270490160650", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"4024931311275503826435792708", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"5363964879812685060522408570", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"413851047162105459574532340", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"551533010323039377059444850", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"827702094324210919149064680", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}], "-", 
       RowBox[{"11930371353179612605329120", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "]"}]}], "-", 
       RowBox[{"15899424857836643258074800", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"357681714462281168857800", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"476676993009071207524500", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}]}], ")"}],
      " ", 
     RowBox[{
      SuperscriptBox[
       SubscriptBox["\[Theta]", "3"], "\[Prime]",
       MultilineFunction->None], "[", "t", "]"}]}], "+", 
    RowBox[{"33766", " ", 
     RowBox[{"(", 
      RowBox[{
       RowBox[{"-", "115636334411063810121713"}], "+", 
       RowBox[{"119200714069641172375638", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"158856982758179383418895", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"12256442787481651944990", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"16333975310165236541475", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"24512885574963303889980", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}], "-", 
       RowBox[{"10592954879532108300", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"14117070218831700750", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}]}], ")"}],
      " ", 
     RowBox[{
      SuperscriptBox[
       SubscriptBox["\[Theta]", "4"], "\[Prime]",
       MultilineFunction->None], "[", "t", "]"}]}], "+", 
    RowBox[{"1557372335", " ", 
     RowBox[{"(", 
      RowBox[{
       RowBox[{"-", "208022065270431745"}], "+", 
       RowBox[{"265736739931305804", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"354143320725572910", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"265736739931305804", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], ")"}], 
     " ", 
     RowBox[{
      SuperscriptBox[
       SubscriptBox["\[Theta]", "5"], "\[Prime]",
       MultilineFunction->None], "[", "t", "]"}]}]}], ")"}], "/", 
  "112995149765925000000000000000"}]], "Output",
 CellChangeTimes->{
  3.520211866067136*^9, 3.520212076437168*^9, {3.5202585043744397`*^9, 
   3.52025851417*^9}, 3.520258669168865*^9, 3.5202588112999945`*^9, 
   3.520259099187461*^9, 3.5203392289192266`*^9, 3.52043737354138*^9, 
   3.5204379168174534`*^9, 3.5204436819222918`*^9, 3.520449230129631*^9, 
   3.5204569783388033`*^9, 3.520532873668348*^9, 3.5205336525428967`*^9, {
   3.520533698827544*^9, 3.520533727760199*^9}, 3.520700227091343*^9, 
   3.520713043701494*^9, 3.5207209459524775`*^9, 3.5207210504084516`*^9, 
   3.5207347465638266`*^9, 3.52073524838853*^9, 3.5207876982934875`*^9, 
   3.5207888920017643`*^9, 3.5208972568862915`*^9, 3.5213037477381215`*^9, 
   3.5213038673559637`*^9, 3.5214860658185167`*^9, 3.5214879336503506`*^9, 
   3.5222451319227495`*^9, 3.522246816380095*^9, 3.5222499204934177`*^9, 
   3.5223307363826175`*^9, 3.5223566295027447`*^9, 3.5223569342937164`*^9, 
   3.5224296753688145`*^9, 3.5225104826604958`*^9, 3.5225118978684406`*^9, 
   3.5227081627193418`*^9, 3.5227751714727554`*^9, 3.5227768653569813`*^9, 
   3.5230286079400415`*^9, 3.5230313574223027`*^9, 3.5235572883124905`*^9, 
   3.523724889884546*^9, 3.5237265812232847`*^9, 3.523983760917122*^9, 
   3.523985008005451*^9, 3.523989322982254*^9, 3.5239903470068245`*^9, 
   3.5239920402676735`*^9, 3.523992152226077*^9, 3.523994833751452*^9, 
   3.5239949436657386`*^9, 3.523995054819096*^9, 3.5242481567987013`*^9, 
   3.5242485438358383`*^9, 3.5242500227322626`*^9, 3.5244001540110693`*^9, 
   3.5244003269608235`*^9, 3.524406303778967*^9, 3.5244065745553455`*^9, 
   3.524406803762315*^9, 3.5244184168180146`*^9, 3.5244187247993183`*^9, 
   3.5244222484800186`*^9, 3.5244223356950073`*^9, 3.524422776451217*^9, 
   3.5244238664325604`*^9, 3.524442240925521*^9, 3.5244427666135893`*^9, 
   3.5244438737929163`*^9, 3.5244457113690195`*^9, 3.5244458890501823`*^9, 
   3.524446902178668*^9, 3.524502455081913*^9, 3.52451197436874*^9, 
   3.524588414954198*^9, 3.525090302809466*^9, 3.5250917107493706`*^9, 
   3.525091967552128*^9, 3.5250922061067147`*^9, 3.5250924614848557`*^9, 
   3.525092581585046*^9, 3.525092705418212*^9, 3.5250929093635635`*^9, 
   3.525093228033149*^9, 3.5250987877541466`*^9, 3.5254480091990952`*^9, 
   3.525461232036228*^9, 3.525518289713848*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<z_comp_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"zcomp", " ", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.520258677882364*^9, 3.520258688153951*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"z1calc", " ", "=", " ", 
   RowBox[{"First", "[", 
    RowBox[{"c", ".", "q"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"z2calc", " ", "=", " ", 
   RowBox[{"First", "[", 
    RowBox[{"c", ".", "dq"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{"zcalc", " ", "=", " ", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{"{", "z1calc", "}"}], ",", 
    RowBox[{"{", "z2calc", "}"}]}], "}"}]}]}], "Input",
 CellChangeTimes->{{3.520272923801213*^9, 3.5202729561930656`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      FractionBox[
       RowBox[{"152273", " ", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "250000"]}], "-", 
     FractionBox[
      RowBox[{"32639", " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "125000"]}], "}"}], 
   ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      FractionBox[
       RowBox[{"152273", " ", 
        RowBox[{
         SuperscriptBox[
          SubscriptBox["\[Theta]", "1"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}]}], "250000"]}], "-", 
     FractionBox[
      RowBox[{"32639", " ", 
       RowBox[{
        SuperscriptBox[
         SubscriptBox["\[Theta]", "2"], "\[Prime]",
         MultilineFunction->None], "[", "t", "]"}]}], "125000"]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVx1tMknEYgHE85GhrwrLDyiSlFtkqJ3ZRiOQf1NwYYs4E2VeY2pwuU5th
gK0cttbUsQErBoUZKtbMCA+ttlwHm60DY0INhyJ52PTbrIuK1G5634tnvz1p
lQ0lF2IYDEYpVBMeOVUloMm+jh6nAbwqGDUOg3EVTj86M1fcOwYq1R/KX4Bu
ppz7CoyfEWTOgyaB2YReKahZzsqmCbWgPyQAd9+WJIjAw6+Lk9BHpWXv8sDH
RnZsPqhiFznOgUuc9iw1yJ5MTbGAdaxrVpWQJotHm1zo53yhYxgUsl7eGgGT
j7SRAyKaXKzdtY0HplzeYT0IcgPJicVgwDzmocDIgjeIjrubzt4AhbOqKRfo
fSYxDIAV9ZSSeZImP+WN7HRwfipzEd1i2+TrBZ96PLx+MCxn/enPpUkXlfNp
DdRQA451kKng8FsITUrux3jRtKGKFS1Y13K+VgeGlzfn6EH3IFOKblSdsLWC
vr6oC93+PDSKVqfapWtgW/SfHo3XTx9bB2V7iUEspkk0MWJFJ85sGPJBkcjf
h3Z81b5FQ83Jl6TgTb/NglKzbJkM7Bz/q0C/xbGMKL9QvrMI385hK0D7RuA0
mtQtmEHbE1ffKMHcHm4Q5Tsny8rBO+olTTMY9H3ZowUTtkYaHGBlY+i6WUKT
gs7pDAuoEf6So60fJ+pR2293F/pgbvgeatmfMYiuZD95j0rjDKvoQ5O29y74
fYiXLs6jia4x1lEI/sg4rqsG/wO5E0J5
  "]]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<z_calc_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"zcalc", " ", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5202729882648997`*^9, 3.5202729939072227`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{"Dc", "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    SubscriptBox["\[PartialD]", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
    RowBox[{"Flatten", "[", 
     RowBox[{"c", ".", "q"}], "]"}]}], "]"}]}], "\[IndentingNewLine]", 
 RowBox[{"DLfc", "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    SubscriptBox["\[PartialD]", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
    RowBox[{"Flatten", "[", 
     RowBox[{"Dc", ".", "d\[Chi]"}], "]"}]}], "]"}]}]}], "Input",
 CellChangeTimes->{{3.520272447276957*^9, 3.5202724889943433`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{"-", 
     FractionBox["152273", "250000"]}], ",", 
    RowBox[{"-", 
     FractionBox["32639", "125000"]}], ",", "0", ",", "0", ",", "0", ",", "0",
     ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}], "}"}]], "Output",
 CellChangeTimes->{{3.5202724533673058`*^9, 3.5202724894163675`*^9}, 
   3.5203392290872364`*^9, 3.520437373694389*^9, 3.5204379175484953`*^9, 
   3.5204436826333323`*^9, 3.520449230295641*^9, 3.5204569784998126`*^9, 
   3.520532873909362*^9, 3.520533652670904*^9, 3.520700227263353*^9, 
   3.5207130443895335`*^9, 3.5207209465955143`*^9, 3.5207210505474596`*^9, 
   3.5207347467348366`*^9, 3.5207352485625396`*^9, 3.5207876984904995`*^9, 
   3.5207888921607733`*^9, 3.520897257069302*^9, 3.521303747969135*^9, 
   3.521303867575976*^9, 3.5214860659355235`*^9, 3.5214879338543625`*^9, 
   3.5222451320657578`*^9, 3.5222468170751348`*^9, 3.5222499207824345`*^9, 
   3.522330737054656*^9, 3.5223566297679453`*^9, 3.522356934512117*^9, 
   3.5224296755608253`*^9, 3.5225104829775133`*^9, 3.5225118981774583`*^9, 
   3.5227081629233537`*^9, 3.5227751715437593`*^9, 3.5227768656019955`*^9, 
   3.523028608245059*^9, 3.523031357614314*^9, 3.523557288508501*^9, 
   3.523724889945549*^9, 3.523726581292289*^9, 3.523983760972125*^9, 
   3.523985008088456*^9, 3.5239893230502577`*^9, 3.523990347076829*^9, 
   3.5239920403486786`*^9, 3.523992152306082*^9, 3.523994833831456*^9, 
   3.523994943746743*^9, 3.5239950549091015`*^9, 3.524248156888706*^9, 
   3.524248543905843*^9, 3.52425002286127*^9, 3.524400154045071*^9, 
   3.5244003270278273`*^9, 3.524406303857971*^9, 3.52440657463235*^9, 
   3.524406803837319*^9, 3.524418416898015*^9, 3.524418724869322*^9, 
   3.5244222485670238`*^9, 3.524422335782012*^9, 3.5244227764772186`*^9, 
   3.5244238665175653`*^9, 3.5244422409775248`*^9, 3.524442766688593*^9, 
   3.5244438738599195`*^9, 3.5244457114350233`*^9, 3.524445889104185*^9, 
   3.5244469022786736`*^9, 3.52450245520592*^9, 3.5245119744677453`*^9, 
   3.5245884150392027`*^9, 3.525090302896471*^9, 3.5250917108353753`*^9, 
   3.525091967639133*^9, 3.5250922061767187`*^9, 3.5250924615698605`*^9, 
   3.5250925816500497`*^9, 3.525092705516217*^9, 3.525092909453563*^9, 
   3.5250932281461554`*^9, 3.525098787833151*^9, 3.525448009270099*^9, 
   3.5254612321112328`*^9, 3.5255182897908525`*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{"{", 
   RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", 
    RowBox[{"-", 
     FractionBox["152273", "250000"]}], ",", 
    RowBox[{"-", 
     FractionBox["32639", "125000"]}], ",", "0", ",", "0", ",", "0"}], "}"}], 
  "}"}]], "Output",
 CellChangeTimes->{{3.5202724533673058`*^9, 3.5202724894163675`*^9}, 
   3.5203392290872364`*^9, 3.520437373694389*^9, 3.5204379175484953`*^9, 
   3.5204436826333323`*^9, 3.520449230295641*^9, 3.5204569784998126`*^9, 
   3.520532873909362*^9, 3.520533652670904*^9, 3.520700227263353*^9, 
   3.5207130443895335`*^9, 3.5207209465955143`*^9, 3.5207210505474596`*^9, 
   3.5207347467348366`*^9, 3.5207352485625396`*^9, 3.5207876984904995`*^9, 
   3.5207888921607733`*^9, 3.520897257069302*^9, 3.521303747969135*^9, 
   3.521303867575976*^9, 3.5214860659355235`*^9, 3.5214879338543625`*^9, 
   3.5222451320657578`*^9, 3.5222468170751348`*^9, 3.5222499207824345`*^9, 
   3.522330737054656*^9, 3.5223566297679453`*^9, 3.522356934512117*^9, 
   3.5224296755608253`*^9, 3.5225104829775133`*^9, 3.5225118981774583`*^9, 
   3.5227081629233537`*^9, 3.5227751715437593`*^9, 3.5227768656019955`*^9, 
   3.523028608245059*^9, 3.523031357614314*^9, 3.523557288508501*^9, 
   3.523724889945549*^9, 3.523726581292289*^9, 3.523983760972125*^9, 
   3.523985008088456*^9, 3.5239893230502577`*^9, 3.523990347076829*^9, 
   3.5239920403486786`*^9, 3.523992152306082*^9, 3.523994833831456*^9, 
   3.523994943746743*^9, 3.5239950549091015`*^9, 3.524248156888706*^9, 
   3.524248543905843*^9, 3.52425002286127*^9, 3.524400154045071*^9, 
   3.5244003270278273`*^9, 3.524406303857971*^9, 3.52440657463235*^9, 
   3.524406803837319*^9, 3.524418416898015*^9, 3.524418724869322*^9, 
   3.5244222485670238`*^9, 3.524422335782012*^9, 3.5244227764772186`*^9, 
   3.5244238665175653`*^9, 3.5244422409775248`*^9, 3.524442766688593*^9, 
   3.5244438738599195`*^9, 3.5244457114350233`*^9, 3.524445889104185*^9, 
   3.5244469022786736`*^9, 3.52450245520592*^9, 3.5245119744677453`*^9, 
   3.5245884150392027`*^9, 3.525090302896471*^9, 3.5250917108353753`*^9, 
   3.525091967639133*^9, 3.5250922061767187`*^9, 3.5250924615698605`*^9, 
   3.5250925816500497`*^9, 3.525092705516217*^9, 3.525092909453563*^9, 
   3.5250932281461554`*^9, 3.525098787833151*^9, 3.525448009270099*^9, 
   3.5254612321112328`*^9, 3.525518289796853*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfc_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"DLfc", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5202725242553596`*^9, 3.520272527959572*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"h", "/.", "zcond"}]], "Input",
 CellChangeTimes->{
  3.5206998450294905`*^9, {3.520700278162264*^9, 3.52070028735379*^9}, {
   3.5207003710585775`*^9, 3.5207003730426908`*^9}}],

Cell[BoxData[
 RowBox[{
  FractionBox["1", "250000"], 
  RowBox[{"127", " ", 
   RowBox[{"(", 
    RowBox[{
     RowBox[{"514", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"3", ",", "5"}], "]"}], "+", 
        RowBox[{
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "1"}], "]"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "3"}], "]"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "+", 
        RowBox[{
         RowBox[{"1", "/", "152273"}], 
         RowBox[{"2", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"-", "32639"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "5"}], "]"}], "+", 
               RowBox[{
                SuperscriptBox["\[ExponentialE]", 
                 FractionBox[
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"3", ",", "4"}], "]"}], " ", 
                   RowBox[{"(", 
                    RowBox[{
                    RowBox[{"p", "[", "1", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                  RowBox[{"a", "[", 
                   RowBox[{"1", ",", "1"}], "]"}]]], " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"3", ",", "1"}], "]"}], " ", 
                   RowBox[{"Cos", "[", 
                    FractionBox[
                    RowBox[{
                    RowBox[{"a", "[", 
                    RowBox[{"3", ",", "2"}], "]"}], " ", 
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                    RowBox[{"a", "[", 
                    RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"3", ",", "3"}], "]"}], " ", 
                   RowBox[{"Sin", "[", 
                    FractionBox[
                    RowBox[{
                    RowBox[{"a", "[", 
                    RowBox[{"3", ",", "2"}], "]"}], " ", 
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                    RowBox[{"a", "[", 
                    RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], 
              ")"}]}], "-", 
            RowBox[{"125000", " ", 
             RowBox[{
              SubscriptBox["z", "1"], "[", "t", "]"}]}]}], ")"}]}]}]}], 
       "]"}]}], "+", 
     RowBox[{"685", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{"1", "/", "152273"}], 
        RowBox[{"2", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"-", "32639"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"a", "[", 
               RowBox[{"3", ",", "5"}], "]"}], "+", 
              RowBox[{
               SuperscriptBox["\[ExponentialE]", 
                FractionBox[
                 RowBox[{
                  RowBox[{"a", "[", 
                   RowBox[{"3", ",", "4"}], "]"}], " ", 
                  RowBox[{"(", 
                   RowBox[{
                    RowBox[{"p", "[", "1", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                 RowBox[{"a", "[", 
                  RowBox[{"1", ",", "1"}], "]"}]]], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{
                  RowBox[{"a", "[", 
                   RowBox[{"3", ",", "1"}], "]"}], " ", 
                  RowBox[{"Cos", "[", 
                   FractionBox[
                    RowBox[{
                    RowBox[{"a", "[", 
                    RowBox[{"3", ",", "2"}], "]"}], " ", 
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                    RowBox[{"a", "[", 
                    RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
                 RowBox[{
                  RowBox[{"a", "[", 
                   RowBox[{"3", ",", "3"}], "]"}], " ", 
                  RowBox[{"Sin", "[", 
                   FractionBox[
                    RowBox[{
                    RowBox[{"a", "[", 
                    RowBox[{"3", ",", "2"}], "]"}], " ", 
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                    RowBox[{"a", "[", 
                    RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], 
             ")"}]}], "-", 
           RowBox[{"125000", " ", 
            RowBox[{
             SubscriptBox["z", "1"], "[", "t", "]"}]}]}], ")"}]}]}], "]"}]}], 
     "-", 
     RowBox[{"514", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "5"}], "]"}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"3", ",", "5"}], "]"}], "+", 
        RowBox[{
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "1"}], "]"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"2", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "3"}], "]"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"2", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "-", 
        RowBox[{
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "1"}], "]"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "3"}], "]"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "+", 
        RowBox[{
         RowBox[{"1", "/", "1199"}], 
         RowBox[{"685", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "5"}], "]"}], "+", 
            RowBox[{
             SuperscriptBox["\[ExponentialE]", 
              FractionBox[
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"3", ",", "4"}], "]"}], " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"p", "[", "1", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]]], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"3", ",", "1"}], "]"}], " ", 
                RowBox[{"Cos", "[", 
                 FractionBox[
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"3", ",", "2"}], "]"}], " ", 
                   RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                  RowBox[{"a", "[", 
                   RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"3", ",", "3"}], "]"}], " ", 
                RowBox[{"Sin", "[", 
                 FractionBox[
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"3", ",", "2"}], "]"}], " ", 
                   RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                  RowBox[{"a", "[", 
                   RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], 
           ")"}]}]}], "-", 
        RowBox[{
         RowBox[{"1", "/", "1199"}], 
         RowBox[{"685", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "5"}], "]"}], "+", 
            RowBox[{
             SuperscriptBox["\[ExponentialE]", 
              FractionBox[
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"4", ",", "4"}], "]"}], " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"p", "[", "1", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]]], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"4", ",", "1"}], "]"}], " ", 
                RowBox[{"Cos", "[", 
                 FractionBox[
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"4", ",", "2"}], "]"}], " ", 
                   RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                  RowBox[{"a", "[", 
                   RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"4", ",", "3"}], "]"}], " ", 
                RowBox[{"Sin", "[", 
                 FractionBox[
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"4", ",", "2"}], "]"}], " ", 
                   RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                  RowBox[{"a", "[", 
                   RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], 
           ")"}]}]}], "-", 
        RowBox[{
         RowBox[{"1", "/", "152273"}], 
         RowBox[{"2", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"-", "32639"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "5"}], "]"}], "+", 
               RowBox[{
                SuperscriptBox["\[ExponentialE]", 
                 FractionBox[
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"3", ",", "4"}], "]"}], " ", 
                   RowBox[{"(", 
                    RowBox[{
                    RowBox[{"p", "[", "1", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                  RowBox[{"a", "[", 
                   RowBox[{"1", ",", "1"}], "]"}]]], " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"3", ",", "1"}], "]"}], " ", 
                   RowBox[{"Cos", "[", 
                    FractionBox[
                    RowBox[{
                    RowBox[{"a", "[", 
                    RowBox[{"3", ",", "2"}], "]"}], " ", 
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                    RowBox[{"a", "[", 
                    RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"3", ",", "3"}], "]"}], " ", 
                   RowBox[{"Sin", "[", 
                    FractionBox[
                    RowBox[{
                    RowBox[{"a", "[", 
                    RowBox[{"3", ",", "2"}], "]"}], " ", 
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                    RowBox[{"a", "[", 
                    RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], 
              ")"}]}], "-", 
            RowBox[{"125000", " ", 
             RowBox[{
              SubscriptBox["z", "1"], "[", "t", "]"}]}]}], ")"}]}]}], "-", 
        FractionBox[
         RowBox[{"250000", " ", 
          RowBox[{
           SubscriptBox["z", "1"], "[", "t", "]"}]}], "152273"]}], "]"}]}], 
     "-", 
     RowBox[{"685", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "5"}], "]"}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"3", ",", "5"}], "]"}], "+", 
        RowBox[{"a", "[", 
         RowBox[{"4", ",", "5"}], "]"}], "+", 
        RowBox[{
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "1"}], "]"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"2", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "3"}], "]"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"2", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "-", 
        RowBox[{
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "1"}], "]"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "3"}], "]"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "+", 
        RowBox[{
         RowBox[{"1", "/", "1199"}], 
         RowBox[{"685", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "5"}], "]"}], "+", 
            RowBox[{
             SuperscriptBox["\[ExponentialE]", 
              FractionBox[
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"3", ",", "4"}], "]"}], " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"p", "[", "1", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]]], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"3", ",", "1"}], "]"}], " ", 
                RowBox[{"Cos", "[", 
                 FractionBox[
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"3", ",", "2"}], "]"}], " ", 
                   RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                  RowBox[{"a", "[", 
                   RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"3", ",", "3"}], "]"}], " ", 
                RowBox[{"Sin", "[", 
                 FractionBox[
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"3", ",", "2"}], "]"}], " ", 
                   RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                  RowBox[{"a", "[", 
                   RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], 
           ")"}]}]}], "+", 
        RowBox[{
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "1"}], "]"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "3"}], "]"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "-", 
        RowBox[{
         RowBox[{"1", "/", "1199"}], 
         RowBox[{"685", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "5"}], "]"}], "+", 
            RowBox[{
             SuperscriptBox["\[ExponentialE]", 
              FractionBox[
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"4", ",", "4"}], "]"}], " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"p", "[", "1", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]]], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"4", ",", "1"}], "]"}], " ", 
                RowBox[{"Cos", "[", 
                 FractionBox[
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"4", ",", "2"}], "]"}], " ", 
                   RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                  RowBox[{"a", "[", 
                   RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
               RowBox[{
                RowBox[{"a", "[", 
                 RowBox[{"4", ",", "3"}], "]"}], " ", 
                RowBox[{"Sin", "[", 
                 FractionBox[
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"4", ",", "2"}], "]"}], " ", 
                   RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                  RowBox[{"a", "[", 
                   RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], 
           ")"}]}]}], "-", 
        RowBox[{
         RowBox[{"1", "/", "152273"}], 
         RowBox[{"2", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"-", "32639"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "5"}], "]"}], "+", 
               RowBox[{
                SuperscriptBox["\[ExponentialE]", 
                 FractionBox[
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"3", ",", "4"}], "]"}], " ", 
                   RowBox[{"(", 
                    RowBox[{
                    RowBox[{"p", "[", "1", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                  RowBox[{"a", "[", 
                   RowBox[{"1", ",", "1"}], "]"}]]], " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"3", ",", "1"}], "]"}], " ", 
                   RowBox[{"Cos", "[", 
                    FractionBox[
                    RowBox[{
                    RowBox[{"a", "[", 
                    RowBox[{"3", ",", "2"}], "]"}], " ", 
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                    RowBox[{"a", "[", 
                    RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
                  RowBox[{
                   RowBox[{"a", "[", 
                    RowBox[{"3", ",", "3"}], "]"}], " ", 
                   RowBox[{"Sin", "[", 
                    FractionBox[
                    RowBox[{
                    RowBox[{"a", "[", 
                    RowBox[{"3", ",", "2"}], "]"}], " ", 
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"-", 
                    RowBox[{"p", "[", "1", "]"}]}], "+", 
                    RowBox[{
                    SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
                    RowBox[{"a", "[", 
                    RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], 
              ")"}]}], "-", 
            RowBox[{"125000", " ", 
             RowBox[{
              SubscriptBox["z", "1"], "[", "t", "]"}]}]}], ")"}]}]}], "-", 
        FractionBox[
         RowBox[{"250000", " ", 
          RowBox[{
           SubscriptBox["z", "1"], "[", "t", "]"}]}], "152273"]}], "]"}]}]}], 
    ")"}]}]}]], "Output",
 CellChangeTimes->{
  3.5206998456685266`*^9, {3.520700227305355*^9, 3.5207002464684515`*^9}, 
   3.520700280836417*^9, {3.520700368975458*^9, 3.5207003735267186`*^9}, 
   3.5207130476217184`*^9, 3.5207209466375165`*^9, 3.520721050587462*^9, 
   3.520734747016853*^9, 3.520735248851556*^9, 3.520787698774515*^9, 
   3.5207888924317884`*^9, 3.5208972571243052`*^9, 3.5213037480331383`*^9, 
   3.5213038676219788`*^9, 3.5214860660015273`*^9, 3.521487933903365*^9, 
   3.5222451321007595`*^9, 3.5222468171611395`*^9, 3.52224992088244*^9, 
   3.522330737250667*^9, {3.5223566298303456`*^9, 3.522356639102581*^9}, 
   3.5223569345745173`*^9, 3.5224296755778265`*^9, 3.5225104833005323`*^9, 
   3.522511898234462*^9, 3.5227081629763565`*^9, 3.5227751715797615`*^9, 
   3.5227768656499977`*^9, 3.5230286084480705`*^9, 3.5230313576563168`*^9, 
   3.523557288553504*^9, 3.523724889981551*^9, 3.523726581333291*^9, 
   3.5239837609951267`*^9, 3.5239850081304584`*^9, 3.5239893230832596`*^9, 
   3.5239903471118307`*^9, 3.523992040389681*^9, 3.523992152344084*^9, 
   3.5239948338724585`*^9, 3.5239949437867455`*^9, 3.5239950549521036`*^9, 
   3.5242481569317093`*^9, 3.5242485439408445`*^9, 3.5242500229332743`*^9, 
   3.5244001540620723`*^9, 3.5244003270498285`*^9, 3.5244063038939734`*^9, 
   3.524406574665352*^9, 3.524406803872321*^9, 3.5244184169480147`*^9, 
   3.524418724901324*^9, 3.524422248612026*^9, 3.5244223358260145`*^9, 
   3.52442277649422*^9, 3.524423866560568*^9, 3.5244422410005255`*^9, 
   3.524442766723595*^9, 3.5244438738909216`*^9, 3.524445711465025*^9, 
   3.5244458891291866`*^9, 3.524446902332677*^9, 3.5245024552519226`*^9, 
   3.5245119745177484`*^9, 3.524588415080205*^9, 3.5250903029414735`*^9, 
   3.5250917108753777`*^9, 3.525091967678135*^9, 3.5250922062077208`*^9, 
   3.5250924616078625`*^9, 3.5250925816810513`*^9, 3.52509270556422*^9, 
   3.525092909503563*^9, 3.5250932282081585`*^9, 3.525098787869153*^9, 
   3.525448009311102*^9, 3.525461232138234*^9, 3.525518289847856*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Psi_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Psi", "/.", "Zstatesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5202582707790785`*^9, 3.5202582747863083`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Run", "[", "\"\<perl math2matopt_torso.pl\>\"", 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<buildopt_torso\>\""}],
    "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.515897431926337*^9, {3.5158975074906588`*^9, 3.5158975156601257`*^9}, 
   3.515946262132225*^9, 3.5159464148519597`*^9}],

Cell[BoxData["2"], "Output",
 CellChangeTimes->{{3.520258761318136*^9, 3.520258811501006*^9}, {
   3.520258891254568*^9, 3.5202588929576654`*^9}, 3.5202590993714714`*^9, 
   3.520270067025814*^9, 3.5202725735641804`*^9, 3.5202730163215046`*^9, 
   3.520273177158704*^9, 3.5202741385366917`*^9, 3.520339229218244*^9, 
   3.520437373791394*^9, 3.5204379177195053`*^9, 3.5204436828043423`*^9, 
   3.5204492304326487`*^9, 3.5204569786118193`*^9, 3.520457212630204*^9, 
   3.520532874182377*^9, 3.5205329974374266`*^9, 3.520533653389945*^9, 
   3.5205337384658113`*^9, 3.5207002274373627`*^9, 3.520713048279756*^9, 
   3.5207209474625635`*^9, 3.520721050690468*^9, 3.52073474714186*^9, 
   3.520735248955562*^9, 3.5207876988875217`*^9, 3.5207888925317945`*^9, 
   3.520897257253313*^9, 3.5213037482241497`*^9, 3.5213038678099895`*^9, 
   3.5214860662165394`*^9, 3.521487934027372*^9, 3.522245132225767*^9, 
   3.522246817473158*^9, 3.5222499212934637`*^9, 3.522330738020711*^9, 
   3.522356630001946*^9, 3.522356934699317*^9, 3.5224296757328353`*^9, 
   3.522510483651552*^9, 3.522511898474476*^9, 3.5227081632063694`*^9, 
   3.522775171687768*^9, 3.5227768658200073`*^9, 3.523028608637081*^9, 
   3.5230313577813234`*^9, 3.5235572887335143`*^9, 3.5237248900815573`*^9, 
   3.5237265814342966`*^9, 3.5239837610991325`*^9, 3.5239850082514653`*^9, 
   3.523989323195266*^9, 3.5239903472248373`*^9, 3.5239920405126877`*^9, 
   3.5239921524620905`*^9, 3.5239948339934654`*^9, 3.523994943914753*^9, 
   3.5239950550661106`*^9, 3.5242481570577164`*^9, 3.5242485440478506`*^9, 
   3.52425002320429*^9, 3.5244001542160807`*^9, 3.524400327178836*^9, 
   3.524406304035981*^9, 3.5244065748113604`*^9, 3.524406804000328*^9, 
   3.524416257032984*^9, 3.524418417068015*^9, 3.5244187250283313`*^9, 
   3.524422248742034*^9, 3.524422335958022*^9, 3.524422776591225*^9, 
   3.5244238666875753`*^9, 3.524442241118532*^9, 3.5244427668496027`*^9, 
   3.5244438740089283`*^9, 3.524445711588032*^9, 3.5244458892471933`*^9, 
   3.5244469025126867`*^9, 3.524502455443934*^9, 3.52451197471976*^9, 
   3.5245884151982117`*^9, 3.5250903030674806`*^9, 3.5250917109923844`*^9, 
   3.5250919677881403`*^9, 3.5250922063167267`*^9, 3.525092461715869*^9, 
   3.525092581780057*^9, 3.5250927056832266`*^9, 3.5250929096035633`*^9, 
   3.525093228432172*^9, 3.525098788037163*^9, 3.5254480096031184`*^9, 
   3.5254612322492404`*^9, 3.525518290074869*^9}]
}, Open  ]]
}, Closed]],

Cell[CellGroupData[{

Cell["Inverse Kinematics", "Section",
 CellChangeTimes->{{3.51353232609383*^9, 3.513532329542585*^9}, {
  3.5140469816745305`*^9, 3.5140469835466375`*^9}, {3.51405491908371*^9, 
  3.514054919326081*^9}, {3.5207282376405377`*^9, 3.520728240214685*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<buildopt_torso\>\""}],
    "]"}], ";"}]], "Input",
 CellChangeTimes->{{3.5158974201226616`*^9, 3.515897422208781*^9}, 
   3.5159409950429645`*^9}],

Cell["define the reset map", "Text",
 CellChangeTimes->{{3.514054915065277*^9, 3.514054918068027*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[CapitalDelta]", "=", " ", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"1", ",", "1", ",", "1", ",", 
       RowBox[{"-", "1"}], ",", 
       RowBox[{"-", "1"}]}], "}"}], ",", "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", 
     "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0", ",", "1", ",", "0"}], "}"}], ",", 
     "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", 
     "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", "1", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}],
   ";"}]], "Input",
 CellChangeTimes->{{3.5126870727453375`*^9, 3.5126871090532956`*^9}, {
   3.512687198656415*^9, 3.5126872022367573`*^9}, {3.5126906830481005`*^9, 
   3.5126907051903095`*^9}, {3.514054842423344*^9, 3.514054877019777*^9}, 
   3.514054909726557*^9, {3.5158588592522273`*^9, 3.5158589038437777`*^9}, {
   3.5158839903865247`*^9, 3.5158839971019087`*^9}, 3.515940988613597*^9, {
   3.515941980195312*^9, 3.515942020051592*^9}}],

Cell["define the guard conditions for pre- and post-impact", "Text",
 CellChangeTimes->{{3.5133462255725746`*^9, 3.5133462845249467`*^9}, {
  3.514049805517045*^9, 3.5140498067311144`*^9}, {3.514055491715686*^9, 
  3.514055499729824*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"initialtime", "=", 
   RowBox[{"{", 
    RowBox[{"t", "\[Rule]", "0"}], "}"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5158840455366793`*^9, 3.515884055622256*^9}, {
  3.5158841551359477`*^9, 3.5158841624563665`*^9}, {3.515891350156479*^9, 
  3.515891350585504*^9}}],

Cell["compute the position of the hip post-impact", "Text",
 CellChangeTimes->{{3.514055461182718*^9, 3.514055471246761*^9}, {
  3.514057296246017*^9, 3.514057317686638*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"resetmap", "=", 
  RowBox[{"Module", "[", 
   RowBox[{
    RowBox[{"{", "newq", "}"}], ",", "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"newq", "=", 
      RowBox[{"\[CapitalDelta]", ".", 
       RowBox[{"(", "q", ")"}]}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "0", "]"}], "->", 
        RowBox[{"newq", "[", 
         RowBox[{"[", 
          RowBox[{"i", ",", "1"}], "]"}], "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", 
         RowBox[{"Length", "[", "q", "]"}]}], "}"}]}], "]"}]}]}], 
   "\[IndentingNewLine]", "]"}]}]], "Input",
 CellChangeTimes->{
  3.515889034698043*^9, {3.5158896543664856`*^9, 3.515889655282538*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.5158896559905787`*^9, 3.515891312993354*^9, 3.5158913534266663`*^9, 
   3.515894251635434*^9, 3.5158942862024117`*^9, 3.515894326961743*^9, 
   3.515894727589658*^9, 3.5158951402502604`*^9, {3.5158951959184446`*^9, 
   3.5158952146955185`*^9}, {3.5158958851218643`*^9, 3.515895898378623*^9}, 
   3.5158967997871804`*^9, 3.5158976253243985`*^9, 3.515941099709951*^9, 
   3.515941360918891*^9, 3.515942032578308*^9, 3.5159427206966662`*^9, {
   3.515942803684413*^9, 3.5159428203423653`*^9}, 3.51594611064756*^9, 
   3.515972658638178*^9, 3.5159857720962243`*^9, 3.516707085509482*^9, 
   3.5167098030309153`*^9, 3.516711289967963*^9, 3.5167126255793557`*^9, 
   3.516731910470809*^9, 3.5167515643706264`*^9, 3.5176792332544694`*^9, 
   3.517679752981196*^9, 3.5176800876443377`*^9, 3.5176859975213633`*^9, 
   3.5176980467735415`*^9, 3.51769848436257*^9, 3.5176985979950695`*^9, 
   3.520728267121224*^9, 3.5207285463751965`*^9, 3.5208973235081024`*^9, 
   3.52130401974968*^9, 3.5214860662895436`*^9, 3.521487934091376*^9, 
   3.52224513228377*^9, 3.5222468175321608`*^9, 3.5222499213874693`*^9, 
   3.5223307381127167`*^9, 3.522356630079946*^9, 3.522356934777317*^9, 
   3.5224296757648373`*^9, 3.5225104836975546`*^9, 3.522511209772084*^9, 
   3.5225118985574803`*^9, 3.522708163285374*^9, 3.5227751717417707`*^9, 
   3.522776865876011*^9, 3.5230286087460876`*^9, 3.5230313578373265`*^9, 
   3.5235572887965183`*^9, 3.5237248901425605`*^9, 3.5237265814943004`*^9, 
   3.523983761149135*^9, 3.5239850083154693`*^9, 3.523989323250269*^9, 
   3.5239903472678394`*^9, 3.5239920405716915`*^9, 3.5239921525100937`*^9, 
   3.5239948340604696`*^9, 3.5239949439737563`*^9, 3.5239950551251135`*^9, 
   3.5242481571317205`*^9, 3.5242485440988536`*^9, 3.524250023281294*^9, 
   3.524400154233082*^9, 3.524400327199837*^9, 3.5244063041029854`*^9, 
   3.5244065748633633`*^9, 3.5244068040493307`*^9, 3.524418417138015*^9, 
   3.5244187250733337`*^9, 3.524422248811038*^9, 3.5244223360330267`*^9, 
   3.524422776608226*^9, 3.5244238667405777`*^9, 3.5244422411595345`*^9, 
   3.524442766919606*^9, 3.5244438740479307`*^9, 3.5244457116200333`*^9, 
   3.524445889284196*^9, 3.5244469025996923`*^9, 3.5245024555219383`*^9, 
   3.5245119747777634`*^9, 3.524588415258215*^9, 3.5250903031414847`*^9, 
   3.5250917110573883`*^9, 3.525091967846143*^9, 3.525092206356729*^9, 
   3.5250924617768726`*^9, 3.525092581818059*^9, 3.5250927057322297`*^9, 
   3.5250929096535635`*^9, 3.5250932285091763`*^9, 3.525098788102166*^9, 
   3.5254480096751223`*^9, 3.5254612322982435`*^9, 3.525518290128872*^9, 
   3.5258245489251366`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["Parameterized Time Using the Output of the Hip", "Subsection",
 CellChangeTimes->{{3.5133462255725746`*^9, 3.513346244405652*^9}, {
  3.51334632129305*^9, 3.513346341733219*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{"phipcondition", "=", 
  RowBox[{
   RowBox[{
    RowBox[{"{", 
     RowBox[{
      RowBox[{"p", "[", "1", "]"}], "\[Rule]", 
      SubscriptBox["deltap", "hip"]}], "}"}], "/.", "constsubs"}], "/.", 
   RowBox[{"t", "\[Rule]", "0"}]}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"phipcondition", "=", 
    RowBox[{
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "2"}], "]"}], "\[Rule]", 
        SubscriptBox["deltap", "hip"]}], "}"}], "/.", "constsubs"}], "/.", 
     RowBox[{"t", "\[Rule]", "0"}]}]}], "*)"}]}]}], "Input",
 CellChangeTimes->{{3.515889672571527*^9, 3.515889674968664*^9}, {
   3.515890444251664*^9, 3.515890444569683*^9}, {3.515942148250924*^9, 
   3.515942174667435*^9}, {3.5159426119234447`*^9, 3.5159426332196627`*^9}, 
   3.515943242212495*^9, 3.515943756731924*^9, {3.5159855129044*^9, 
   3.5159855341526146`*^9}, {3.5207282787238874`*^9, 
   3.5207282828571243`*^9}, {3.5223565377914543`*^9, 
   3.5223565440782657`*^9}, {3.5224294884521236`*^9, 3.5224294923883486`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"p", "[", "1", "]"}], "\[Rule]", 
   RowBox[{"-", 
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"685", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], "]"}]}]}], 
       ")"}]}], "250000"]}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.5158892342924585`*^9, {3.5158896677502513`*^9, 3.5158896815790424`*^9}, 
   3.5158904450737114`*^9, 3.5158913168935766`*^9, 3.515891355118763*^9, 
   3.515894251663436*^9, 3.5158942862234125`*^9, 3.5158943269927444`*^9, 
   3.515894727617659*^9, 3.5158951402732615`*^9, {3.5158951959444456`*^9, 
   3.5158952147145195`*^9}, {3.5158958851388655`*^9, 3.5158958984006243`*^9}, 
   3.515896799816182*^9, 3.5158976253443995`*^9, 3.5159411065893445`*^9, 
   3.515941363390033*^9, 3.515942035144455*^9, 3.515942176708552*^9, {
   3.515942621377986*^9, 3.5159426337216916`*^9}, {3.515942707017884*^9, 
   3.5159427366405783`*^9}, 3.5159428217274446`*^9, 3.5159432426405196`*^9, 
   3.515943757306957*^9, 3.5159461106765623`*^9, 3.51597265868318*^9, 
   3.5159857721222258`*^9, 3.5167070855404835`*^9, 3.5167098031069193`*^9, 
   3.516711290038967*^9, 3.5167126256843615`*^9, 3.5167319105188117`*^9, 
   3.5167515644066286`*^9, 3.5176792336174903`*^9, 3.5176797530431995`*^9, 
   3.517680087718342*^9, 3.517685997596368*^9, 3.517698046827544*^9, 
   3.517698484413573*^9, 3.5176985980660734`*^9, 3.520728283507161*^9, 
   3.5207285463971977`*^9, 3.5208973235321035`*^9, 3.521304019769681*^9, 
   3.521486066339546*^9, 3.521487934123378*^9, 3.522245132312772*^9, 
   3.522246817553162*^9, 3.522249921431472*^9, 3.5223307381467185`*^9, 
   3.5223566301735463`*^9, 3.5223569348085175`*^9, 3.5224296757768383`*^9, 
   3.522510483726557*^9, 3.522511209826087*^9, 3.5225118986154833`*^9, 
   3.522708163317376*^9, 3.5227751717677727`*^9, 3.5227768659130125`*^9, 
   3.5230286088150916`*^9, 3.523031357862328*^9, 3.5235572888235197`*^9, 
   3.5237248901755624`*^9, 3.5237265815333023`*^9, 3.5239837611731367`*^9, 
   3.523985008354471*^9, 3.523989323277271*^9, 3.5239903472948413`*^9, 
   3.523992040610693*^9, 3.523992152552096*^9, 3.5239948341104727`*^9, 
   3.5239949440117583`*^9, 3.523995055164116*^9, 3.524248157171723*^9, 
   3.5242485441318555`*^9, 3.5242500233182964`*^9, 3.5244001542390823`*^9, 
   3.5244003272168384`*^9, 3.524406304148988*^9, 3.5244065749033656`*^9, 
   3.5244068040813327`*^9, 3.524418417178015*^9, 3.524418725102335*^9, 
   3.5244222488550406`*^9, 3.524422336079029*^9, 3.5244227766152267`*^9, 
   3.5244238667775803`*^9, 3.5244422411785355`*^9, 3.5244427669546084`*^9, 
   3.524443874074932*^9, 3.5244457116500354`*^9, 3.5244458893091965`*^9, 
   3.5244469026336937`*^9, 3.52450245555694*^9, 3.5245119748137655`*^9, 
   3.524588415294217*^9, 3.525090303180487*^9, 3.5250917110953903`*^9, 
   3.5250919678861446`*^9, 3.525092206388731*^9, 3.5250924618118744`*^9, 
   3.5250925818710623`*^9, 3.5250927057752323`*^9, 3.525092909673564*^9, 
   3.525093228558179*^9, 3.525098788126168*^9, 3.525448009704124*^9, 
   3.5254612323242445`*^9, 3.525518290159874*^9, 3.525824548948138*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData["resetmap"], "Input"],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwV0l9Ik2EYBfCNLEaWbWV5sbkK8bONhmFmMJ32fm5mrW26aK6xJCQrS7eG
o/qmoLFSqNTQQWvJulmboFc6LdYCK1t/pFqRsXDRzXD1qhCB4Iyo57k4/C4P
HM7uJruxmc/j8fQQh0C6NrqfEqGpIn7pACX9oQKRA8y5nVGiicBkvhNs7Er+
40CG7S9zgQu3ZtTobPvyfVJOieGILoqqBx2FHrB7LxexVFCi1B/zT4CPXs30
hsFE7CZhKimRGUpyi8C6HcXePeBSjyanDqzti41bwReN8wn0bD53qhtsKLz6
KQRafpUrRsDk8RNudKO8zSyooqTMxAll4GC6KoUaWUk8AMat0aIgmJKKV4KH
KDnfUTO7Csp3jvsz4PB2puQKoUT8eNN7VDHX8pMDl6wXWlyg/m2uqgOc+LhV
iyb4Gl8n6Jvmj6AD9oVJ1BkLaldBY+X6TvSLarE0A/Z8OOxmWdjhYtqLWlUb
rmvA00+/PUQ333E/R/9kGJsW7C0IeNBWt0SnA2lsnRkNx7cNoFtEljw9KDjK
CBvAa77v9ehXZXUSHYusPDODinvyBMrY5kwnsf/178tO8MeueQkHSiPU7gc/
M+muoWpKzmWnij3gvtG/BrTd9q4NbRU96UOnw9Fh9A2jHEO97qmXKKsbWkYX
824E7oJNzQdlrBr+lJXtrwVD4hrXGdBc+iBrDfwPmPUmgg==
  "]]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"phipcondition", "/.", "resetmap"}]], "Input",
 CellChangeTimes->{{3.516751800532134*^9, 3.516751805196401*^9}, {
  3.516752237310117*^9, 3.5167522378981504`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"p", "[", "1", "]"}], "\[Rule]", 
   RowBox[{"-", 
    RowBox[{
     FractionBox["1", "250000"], 
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{"685", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}]}]}]}], "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVyGtIk2EYgOElpYFjGNO5HxoUuM1Ek9xMptbeOWewdkhK17QDHcROrsMo
vzVScOUgYkGrRMYKWbbYjJpZIZNw0BCEIVH5JWNYM8y3jPqhfjPQnufHzQX3
thPWxtMbeDyeEVLEX64EKygR32dLQ2D78bqpiwpKngeLtlxCV/8p0U7h20Ib
6LIk1xmw2+SutIPRLzENmgxu95JqSuYqDRGU7bpc5AFNaWbUUkPJRMroGwbX
pLHeV2BN6DaR7KHkQXZFrhSUt5X3ycCUUyswgUH+RLgVtBxNsGj+XeZIN5ir
YT48Bf+Ea0sDoErQ3IN6VB3mzXsp2V1izykGX4jIHJp5bOuUH8xQjkkHwU5+
wdKgipJ+c8MkB75WDPvS4Oxjya5rhJL1LEEc7U2dXWDAcOO5M3YwwObVXgcT
00IdGpNp+x0gXckIoLZ38yOoWRTQcaBYnulAl87/kqdBB9fQo1ZTYvg934cO
2bKc9eDoZPIJ+vGmM4o+I7IOHeji/B50MVqo14Nx0SYzav8rdKNlqpZ8A6j/
JMlpBluFXw+gfLEmgQ65l8fNYFV9CYvOjHxuOgxOu9JXbaC3PVHA4Gd/Wn3g
obYfXffqKNnf8n2nB4yMrxnR2R3xC6isPHIHPTgw5kWTN6pDqFX65j1qiHsW
0W95t/wPwZPKqmK1hpIrC9m+feAMp7WfApe5RxtXwf/1wTVs
  "]]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "\[InvisibleSpace]", 
  "fb"}]], "Input"],

Cell[BoxData[
 FractionBox[
  RowBox[{
   RowBox[{"-", 
    RowBox[{"p", "[", "1", "]"}]}], "-", 
   FractionBox[
    RowBox[{"127", " ", 
     RowBox[{"(", 
      RowBox[{
       RowBox[{"685", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
       RowBox[{"514", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
      ")"}]}], "250000"]}], 
  RowBox[{"a", "[", 
   RowBox[{"1", ",", "1"}], "]"}]]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVzF9IU2EYgPFVMr0IsxgK5Uwv2mmoSLqM3HR+a22HhseUcqdYf9AKLHWZ
Q93JqDCzi0XSZknY8GI6a6JSK2K0oEhDoiFFtQunY41oX+muos1o630vHn53
T1GzufHsBpFIxEHixKakp4KSIdf5xYt7KQnMMFs7Qb42VYUOtPqkFrBAGk5b
QZ1uqFIAq/IWtOjzU45RoqSk8Cf3Eo0Wd+1ygGt+wXdcRcmyssH5FHwrfTfo
BWuu24isBnRXSBjwyPCekd1g7yV99mEw27bwxAT6Q6EgqqDWE9dA1SHhoxs8
qVWXToLbB/l+lL1l5rPUlHyJCzlyUFWqiaKylsJFFziW8jMToCeW/3uilpJU
Nfs+Aa4ovc4kyJQw5T2Ekm/GLQF0NnYhZgWjZW2tAqiI51ZfBj99lhhQb73+
QR8Y0GdMorLM2DO0veORIQEuyzP70Ih9VZEEu4rYfo2GkubOHyNoypN14yDY
TVfGUXHLwBu0bZ+8wwBya+MOdOOOnXV1YAEn5lH1Nskd1NdkyuNA+2kmxwiG
GiMN6FJUu4ReMf15zYPTweIgavv3tekY6HCvd1vwPxvKt4Ln4r/MTtAyHLtq
P0BJmP1e5gB7w+l6NJIOtKO5Rv9t9GjPq1H07rxqCi2XvphD5yvvraK6vzdd
98EEs1+u0VIy83CzkwUfz+mFM2Diw1jGOvgf7eYv/w==
  "]]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"tau", "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "\[InvisibleSpace]", 
        "fb"}], "/.", "\[InvisibleSpace]", "phipcondition"}], "/.", 
      "resetmap"}], "/.", "constsubs"}], "]"}]}], 
  "\[IndentingNewLine]"}]], "Input",
 CellChangeTimes->{{3.5158895462843037`*^9, 3.5158895925569506`*^9}, {
  3.515889789857235*^9, 3.5158897931634245`*^9}, {3.515889831234602*^9, 
  3.5158898935841684`*^9}, {3.5158904699201326`*^9, 3.5158904714572206`*^9}, {
  3.515891377005015*^9, 3.515891377386037*^9}, {3.5159855481004124`*^9, 
  3.5159855596250715`*^9}}],

Cell[BoxData[
 RowBox[{
  FractionBox["1", 
   RowBox[{"250000", " ", 
    RowBox[{"a", "[", 
     RowBox[{"1", ",", "1"}], "]"}]}]], 
  RowBox[{"127", " ", 
   RowBox[{"(", 
    RowBox[{
     RowBox[{
      RowBox[{"-", "685"}], " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "-", 
     RowBox[{"514", " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
     RowBox[{"514", " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
     RowBox[{"685", " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
    ")"}]}]}]], "Output",
 CellChangeTimes->{
  3.515889452554943*^9, {3.515889568025547*^9, 3.515889592954973*^9}, 
   3.515889692185649*^9, 3.5158898014698997`*^9, {3.51588983801899*^9, 
   3.5158898676326838`*^9}, 3.515889908068997*^9, {3.5158904521211147`*^9, 
   3.515890472167261*^9}, 3.5158913777660584`*^9, 3.515894251696438*^9, 
   3.5158942863154182`*^9, 3.51589432708775*^9, 3.5158947277126646`*^9, 
   3.515895140365267*^9, {3.515895196040451*^9, 3.5158952148045244`*^9}, {
   3.5158958852288704`*^9, 3.515895898487629*^9}, 3.5158967999501896`*^9, 
   3.5158976254364047`*^9, 3.5159411084254494`*^9, 3.515941365002125*^9, 
   3.515942036213516*^9, 3.515942715326359*^9, {3.515942799867194*^9, 
   3.515942823307535*^9}, 3.5159442992959566`*^9, 3.515946110855572*^9, 
   3.5159726588971925`*^9, 3.515985772270234*^9, 3.516707085705493*^9, 
   3.516709803409937*^9, 3.516711290475992*^9, 3.5167126260073805`*^9, 
   3.5167319108068285`*^9, 3.516751564688645*^9, 3.517679235595603*^9, 
   3.517679753524227*^9, 3.5176800882373714`*^9, 3.5176859981183977`*^9, 
   3.5176980472765703`*^9, 3.5176984847585926`*^9, 3.5176985985751023`*^9, 
   3.520728295358839*^9, 3.5207285464992037`*^9, 3.520897323632109*^9, 
   3.5213040198646865`*^9, 3.5214860665835605`*^9, 3.5214879342553854`*^9, 
   3.5222451324207783`*^9, 3.522246817640167*^9, 3.5222499215924807`*^9, 
   3.522330738269725*^9, 3.522356630345146*^9, 3.522356934980118*^9, 
   3.5224296758298407`*^9, 3.522510484010573*^9, 3.52251121023011*^9, 
   3.522511898821495*^9, 3.522708163525388*^9, 3.522775171867778*^9, 
   3.5227768661080236`*^9, 3.523028609083107*^9, 3.523031357965334*^9, 
   3.5235572889945292`*^9, 3.5237248902855687`*^9, 3.52372658166531*^9, 
   3.5239837612411404`*^9, 3.523985008517481*^9, 3.5239893234032784`*^9, 
   3.523990347406848*^9, 3.5239920407497015`*^9, 3.5239921526831036`*^9, 
   3.5239948342674813`*^9, 3.5239949441737676`*^9, 3.523995055323125*^9, 
   3.524248157332732*^9, 3.5242485442518625`*^9, 3.524250023466305*^9, 
   3.5244001542610836`*^9, 3.524400327281842*^9, 3.5244063043119974`*^9, 
   3.5244065750293727`*^9, 3.52440680419934*^9, 3.5244184173280153`*^9, 
   3.5244187252123413`*^9, 3.524422249012049*^9, 3.5244223362370386`*^9, 
   3.5244227766332273`*^9, 3.5244238669215884`*^9, 3.52444224124854*^9, 
   3.524442767109617*^9, 3.5244438741809382`*^9, 3.5244457117580414`*^9, 
   3.5244458893892016`*^9, 3.524446902790703*^9, 3.5245024558719587`*^9, 
   3.5245119749607735`*^9, 3.5245884154382253`*^9, 3.525090303333496*^9, 
   3.525091711232398*^9, 3.525091968031153*^9, 3.525092206491737*^9, 
   3.525092461953882*^9, 3.525092581984069*^9, 3.5250927059392414`*^9, 
   3.5250929097735634`*^9, 3.525093228739189*^9, 3.525098788242174*^9, 
   3.5254480098121305`*^9, 3.5254612324172497`*^9, 3.5255182902728806`*^9, 
   3.5258245492001524`*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<tau\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"tau", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.515891747597212*^9, 3.5158917482732506`*^9}}],

Cell["extract the (relative degree two) position-based outputs", "Text",
 CellChangeTimes->{{3.51405623270548*^9, 3.514056248417758*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["y", "2"], "=", " ", 
  RowBox[{
   RowBox[{
    RowBox[{"(", 
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "2"}]], "  ", "-", " ", 
      SubscriptBox["y", 
       RowBox[{"d", ",", "2"}]]}], "  ", ")"}], "/.", "\[InvisibleSpace]", 
    "phipcondition"}], "/.", "constsubs"}]}]], "Input",
 CellChangeTimes->{{3.5159429875229278`*^9, 3.515943032690511*^9}, {
  3.51598557480694*^9, 3.5159855753639717`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"], "-", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}], "+", 
        
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}]}], 
       ")"}]}], "+", 
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{"-", "514"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"685", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
      RowBox[{
       RowBox[{"514", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"685", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}]]}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"], "-", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}], "+", 
        
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}]}], 
       ")"}]}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"4", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"], "-", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}], "+", 
        
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}]}], 
       ")"}]}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"5", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"], "-", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}], "+", 
        
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}]}], 
       ")"}]}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.515890267326545*^9, 3.5158905060692005`*^9, 3.51589425173344*^9, 
   3.51589428635242*^9, 3.5158943271307526`*^9, 3.5158947277496667`*^9, 
   3.515895140404269*^9, {3.5158951960774536`*^9, 3.5158952148375263`*^9}, {
   3.5158958852608724`*^9, 3.515895898523631*^9}, 3.5158967999951925`*^9, 
   3.5158976254704065`*^9, 3.5159411188630466`*^9, 3.5159413671962504`*^9, 
   3.515942038314636*^9, 3.515942857994519*^9, {3.5159429911471353`*^9, 
   3.5159430336865683`*^9}, 3.5159443036172037`*^9, 3.5159461108905745`*^9, 
   3.515964447936552*^9, 3.5159726589391947`*^9, 3.515985772310237*^9, 
   3.5167070857444954`*^9, 3.5167098034899416`*^9, 3.516711290560997*^9, 
   3.5167126260793843`*^9, 3.5167319108548307`*^9, 3.5167515647386475`*^9, 
   3.517679235654607*^9, 3.517679753595231*^9, 3.5176800884303827`*^9, 
   3.5176859982004023`*^9, 3.5176980473385735`*^9, 3.5176984848335967`*^9, 
   3.5176985986531067`*^9, 3.52072830236724*^9, 3.5207285465332055`*^9, 
   3.520897323675112*^9, 3.5213040199376907`*^9, 3.521486066642564*^9, 
   3.5214879343343897`*^9, 3.52224513245478*^9, 3.522246817670169*^9, 
   3.522249921646484*^9, 3.522330738307728*^9, 3.522356630391947*^9, 
   3.5223569350269175`*^9, 3.522429675851842*^9, 3.522510484052575*^9, 
   3.5225112102931137`*^9, 3.522511898881499*^9, 3.522708163583391*^9, 
   3.5227751718977795`*^9, 3.5227768661530266`*^9, 3.523028609172112*^9, 
   3.5230313580053363`*^9, 3.5235572890385313`*^9, 3.523724890321571*^9, 
   3.5237265817073126`*^9, 3.523983761264142*^9, 3.523985008560483*^9, 
   3.5239893234342794`*^9, 3.5239903474388494`*^9, 3.523992040788704*^9, 
   3.5239921527191057`*^9, 3.5239948343084836`*^9, 3.52399494421377*^9, 
   3.523995055371128*^9, 3.5242481573757343`*^9, 3.5242485442858644`*^9, 
   3.524250023524308*^9, 3.5244001542760844`*^9, 3.524400327302843*^9, 
   3.5244063043589997`*^9, 3.5244065750633745`*^9, 3.524406804230341*^9, 
   3.5244184173780155`*^9, 3.5244187252423434`*^9, 3.5244222490610523`*^9, 
   3.524422336284041*^9, 3.5244227766462283`*^9, 3.5244238669625907`*^9, 
   3.524442241270541*^9, 3.52444276715562*^9, 3.52444387421094*^9, 
   3.524445711788043*^9, 3.524445889412203*^9, 3.5244469028357058`*^9, 
   3.524502455914961*^9, 3.5245119749967756`*^9, 3.5245884154782276`*^9, 
   3.5250903033754983`*^9, 3.5250917112674*^9, 3.525091968072156*^9, 
   3.525092206523739*^9, 3.5250924619888844`*^9, 3.52509258201307*^9, 
   3.5250927059842443`*^9, 3.5250929098035636`*^9, 3.5250932288011923`*^9, 
   3.525098788275176*^9, 3.5254480098661337`*^9, 3.5254612324482517`*^9, 
   3.525518290383887*^9, 3.5258245492861576`*^9}]
}, Open  ]],

Cell["compute the pre- and post-impact values of the outputs", "Text",
 CellChangeTimes->{{3.514056343774789*^9, 3.514056355919777*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"yplus", "=", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["y", "2"], "/.", "initialtime"}], "/.", "resetmap"}], "//", 
   "Simplify"}]}]], "Input",
 CellChangeTimes->{{3.5159428874992065`*^9, 3.5159429143387423`*^9}, {
  3.51594304105899*^9, 3.5159430447622013`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"2", ",", "5"}], "]"}], "-", 
     FractionBox[
      RowBox[{
       RowBox[{"685", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
       RowBox[{"514", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
      RowBox[{
       RowBox[{"685", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
       RowBox[{"514", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}]]}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"3", ",", "5"}], "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"4", ",", "5"}], "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"5", ",", "5"}], "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{{3.5159428883502555`*^9, 3.515942914923775*^9}, 
   3.5159430466363087`*^9, 3.515943096152141*^9, 3.515944306763384*^9, 
   3.515946111471607*^9, 3.5159644514537535`*^9, 3.5159726595632305`*^9, 
   3.5159857729882755`*^9, 3.5167070864565363`*^9, 3.5167098040589743`*^9, 
   3.51671129078701*^9, 3.5167126263153977`*^9, 3.516731911083844*^9, 
   3.5167515649466596`*^9, 3.5176792358986206`*^9, 3.5176797538342447`*^9, 
   3.517680088689398*^9, 3.517685998558423*^9, 3.517698047567587*^9, 
   3.5176984850736103`*^9, 3.517698598901121*^9, 3.5207283066354837`*^9, 
   3.520728546560207*^9, 3.520897323711114*^9, 3.521304019983693*^9, 
   3.5214860666915665`*^9, 3.521487934370392*^9, 3.522245132486782*^9, 
   3.5222468176981707`*^9, 3.522249921700487*^9, 3.5223307383427296`*^9, 
   3.5223566304387465`*^9, 3.522356935073718*^9, 3.522429675866843*^9, 
   3.522510484249586*^9, 3.5225112106221323`*^9, 3.5225118989345016`*^9, 
   3.522708163625394*^9, 3.5227751719307814`*^9, 3.522776866190029*^9, 
   3.523028609257117*^9, 3.523031358036338*^9, 3.5235572890745335`*^9, 
   3.523724890356573*^9, 3.523726581748315*^9, 3.523983761289143*^9, 
   3.523985008605486*^9, 3.5239893234672813`*^9, 3.5239903474728518`*^9, 
   3.5239920408307056`*^9, 3.523992152759108*^9, 3.523994834351486*^9, 
   3.5239949442567725`*^9, 3.5239950554171305`*^9, 3.5242481574207373`*^9, 
   3.5242485443208666`*^9, 3.5242500235673103`*^9, 3.5244001542920856`*^9, 
   3.524400327328845*^9, 3.524406304406002*^9, 3.5244065750983763`*^9, 
   3.5244068042643433`*^9, 3.524418417438016*^9, 3.5244187252743454`*^9, 
   3.5244222491090546`*^9, 3.5244223363310432`*^9, 3.5244227766602287`*^9, 
   3.5244238670035934`*^9, 3.5244422412955427`*^9, 3.5244427671976223`*^9, 
   3.524443874243942*^9, 3.5244457118200455`*^9, 3.5244458894382043`*^9, 
   3.5244469028727074`*^9, 3.524502455952963*^9, 3.5245119750337777`*^9, 
   3.52458841552223*^9, 3.5250903034155006`*^9, 3.5250917113074026`*^9, 
   3.5250919681141577`*^9, 3.52509220655274*^9, 3.5250924620258865`*^9, 
   3.5250925820460725`*^9, 3.5250927060322466`*^9, 3.525092909833564*^9, 
   3.525093228860196*^9, 3.5250987883101788`*^9, 3.525448009899135*^9, 
   3.5254612324792533`*^9, 3.5255182904268894`*^9, 3.5258245494881687`*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{"\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<y_plus\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"yplus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}]}]}]], "Input",
 CellChangeTimes->{{3.515893576706831*^9, 3.51589357756488*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"yminus", "=", 
  RowBox[{
   SubscriptBox["y", "2"], "/.", "resetmap"}]}]], "Input",
 CellChangeTimes->{{3.5158912072393045`*^9, 3.515891227677474*^9}, {
  3.515943128370984*^9, 3.515943134586339*^9}, {3.515943165683118*^9, 
  3.515943166187147*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             FractionBox[
              RowBox[{"127", " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"685", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                 "+", 
                 RowBox[{"514", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                   "]"}]}]}], ")"}]}], "250000"]}], "+", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}], 
     "+", 
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{"-", "514"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"685", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
      RowBox[{
       RowBox[{"514", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"685", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}]]}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             FractionBox[
              RowBox[{"127", " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"685", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                 "+", 
                 RowBox[{"514", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                   "]"}]}]}], ")"}]}], "250000"]}], "+", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}], 
     "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"4", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             FractionBox[
              RowBox[{"127", " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"685", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                 "+", 
                 RowBox[{"514", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                   "]"}]}]}], ")"}]}], "250000"]}], "+", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}], 
     "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"5", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             FractionBox[
              RowBox[{"127", " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"685", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                 "+", 
                 RowBox[{"514", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                   "]"}]}]}], ")"}]}], "250000"]}], "+", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}], 
     "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{{3.5158912127146177`*^9, 3.5158912281054983`*^9}, 
   3.5158942517994437`*^9, 3.5158942864094234`*^9, 3.515894327207757*^9, 
   3.51589472781367*^9, 3.515895140463272*^9, {3.5158951961434574`*^9, 
   3.5158952148955297`*^9}, {3.515895885310875*^9, 3.5158958986236367`*^9}, 
   3.5158968000671964`*^9, 3.51589762552641*^9, 3.5159411532770147`*^9, 
   3.515941372136533*^9, 3.515942044244975*^9, 3.5159431301160836`*^9, {
   3.515943162203919*^9, 3.5159431667031765`*^9}, 3.5159443146768365`*^9, 
   3.5159461115766134`*^9, 3.515964475797146*^9, 3.5159726596492357`*^9, 
   3.515985773047279*^9, 3.5167070865455413`*^9, 3.5167098042449846`*^9, 
   3.516711290858014*^9, 3.5167126264794073`*^9, 3.5167319111848497`*^9, 
   3.5167515649956627`*^9, 3.5176792364036493`*^9, 3.517679754050257*^9, 
   3.517680088841406*^9, 3.5176859987134314`*^9, 3.517698047746597*^9, 
   3.5176984853146243`*^9, 3.517698599124134*^9, 3.5207283137618914`*^9, 
   3.520728546596209*^9, 3.520897323754116*^9, 3.521304020053697*^9, 
   3.5214860667565703`*^9, 3.521487934416394*^9, 3.522245132521784*^9, 
   3.5222468177561736`*^9, 3.5222499217654905`*^9, 3.5223307383847322`*^9, 
   3.522356630485547*^9, 3.522356935136118*^9, 3.522429675887844*^9, 
   3.5225104843265905`*^9, 3.522511210681136*^9, 3.5225118990025053`*^9, 
   3.5227081636723967`*^9, 3.522775171962784*^9, 3.5227768662330313`*^9, 
   3.5230286093411217`*^9, 3.523031358107342*^9, 3.523557289116536*^9, 
   3.523724890394575*^9, 3.5237265817913175`*^9, 3.5239837613121443`*^9, 
   3.5239850086494884`*^9, 3.5239893235012836`*^9, 3.523990347505853*^9, 
   3.5239920408737087`*^9, 3.52399215280011*^9, 3.523994834396489*^9, 
   3.523994944306775*^9, 3.523995055463133*^9, 3.52424815746474*^9, 
   3.5242485443548684`*^9, 3.524250023671316*^9, 3.524400154307086*^9, 
   3.524400327349846*^9, 3.524406304455005*^9, 3.5244065751323786`*^9, 
   3.5244068042913437`*^9, 3.5244184174880157`*^9, 3.5244187253073473`*^9, 
   3.5244222491590576`*^9, 3.5244223363810463`*^9, 3.52442277667523*^9, 
   3.5244238670445957`*^9, 3.5244422413175435`*^9, 3.5244427672426248`*^9, 
   3.524443874273944*^9, 3.524445711852047*^9, 3.5244458894642057`*^9, 
   3.5244469029207106`*^9, 3.5245024559959655`*^9, 3.5245119750807805`*^9, 
   3.5245884155622325`*^9, 3.5250903034555025`*^9, 3.5250917113474045`*^9, 
   3.5250919681561604`*^9, 3.525092206583742*^9, 3.525092462064889*^9, 
   3.5250925820800743`*^9, 3.5250927060802493`*^9, 3.525092909873564*^9, 
   3.5250932289272*^9, 3.5250987883421803`*^9, 3.5254480099391375`*^9, 
   3.5254612325142555`*^9, 3.5255182904698915`*^9, 3.5258245495211706`*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<y_minus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"yminus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], 
  ";"}], "\[IndentingNewLine]"}], "Input",
 CellChangeTimes->{{3.5158935571417117`*^9, 3.515893586749405*^9}}],

Cell[CellGroupData[{

Cell["solve for the inverse kinematics", "Subsubsection",
 CellChangeTimes->{{3.5133462255725746`*^9, 3.5133462845249467`*^9}, {
  3.5140475842599964`*^9, 3.514047587214165*^9}, {3.514050381757004*^9, 
  3.5140503867632904`*^9}, {3.5140508768053193`*^9, 3.5140508825446477`*^9}, {
  3.514056179159212*^9, 3.51405618309467*^9}}],

Cell["create a function to solve the inverse kinematics", "Text",
 CellChangeTimes->{{3.514058861590675*^9, 3.514058871479416*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"SolveEquations", "[", 
    RowBox[{"equations_", ",", "variables_", ",", "solutionnumbers_"}], "]"}],
    ":=", "\[IndentingNewLine]", 
   RowBox[{"Module", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"i", ",", 
       RowBox[{"solutions", "=", 
        RowBox[{"{", "}"}]}], ",", "tmpeqn", ",", "tmpsolution"}], "}"}], ",",
      "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"For", "[", 
       RowBox[{
        RowBox[{"i", "=", "1"}], ",", 
        RowBox[{"i", "\[LessEqual]", 
         RowBox[{"Length", "[", "variables", "]"}]}], ",", 
        RowBox[{"i", "++"}], ",", "\[IndentingNewLine]", 
        RowBox[{
         RowBox[{"tmpeqn", "=", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{"equations", "[", 
             RowBox[{"[", "i", "]"}], "]"}], ")"}], "\[Equal]", "0"}]}], ";", 
         "\[IndentingNewLine]", 
         RowBox[{"tmpsolution", "=", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{
            "variables", "\[LeftDoubleBracket]", "i", 
             "\[RightDoubleBracket]"}], "\[Rule]", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{
              "variables", "\[LeftDoubleBracket]", "i", 
               "\[RightDoubleBracket]"}], "/.", 
              RowBox[{"Flatten", "[", 
               RowBox[{"{", 
                RowBox[{"solutions", ",", 
                 RowBox[{
                  RowBox[{"Solve", "[", 
                   RowBox[{"tmpeqn", ",", 
                    RowBox[{
                    "variables", "\[LeftDoubleBracket]", "i", 
                    "\[RightDoubleBracket]"}]}], "]"}], 
                  "\[LeftDoubleBracket]", 
                  RowBox[{
                  "solutionnumbers", "\[LeftDoubleBracket]", "i", 
                   "\[RightDoubleBracket]"}], "\[RightDoubleBracket]"}]}], 
                "}"}], "]"}]}], ")"}]}], "}"}]}], ";", "\[IndentingNewLine]", 
         
         RowBox[{"solutions", "=", 
          RowBox[{"Join", "[", 
           RowBox[{"solutions", ",", 
            RowBox[{"(", 
             RowBox[{"tmpsolution", "/.", "solutions"}], ")"}]}], "]"}]}]}]}],
        "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", 
      "solutions"}]}], "\[IndentingNewLine]", "]"}]}], ";"}]], "Input"],

Cell[BoxData[
 RowBox[{
  RowBox[{"atest", "=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "1"}], "]"}], "\[Rule]", "1.19689396035823`"}], ",", 
     
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "2"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "3"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "4"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "5"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "1"}], "]"}], "\[Rule]", "0.20214668746590925`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "2"}], "]"}], "\[Rule]", "7.840374588741148`"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "3"}], "]"}], "\[Rule]", "0.22483245436904015`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "4"}], "]"}], "\[Rule]", 
      RowBox[{"-", "0.8069757285872418`"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "5"}], "]"}], "\[Rule]", "0.18712786224886793`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "1"}], "]"}], "\[Rule]", 
      RowBox[{"-", "0.0828259043763153`"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "2"}], "]"}], "\[Rule]", "13.315981803707265`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "3"}], "]"}], "\[Rule]", "0.20729365656915424`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "4"}], "]"}], "\[Rule]", "4.154819403051306`"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "5"}], "]"}], "\[Rule]", "0.2573791697685864`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "1"}], "]"}], "\[Rule]", 
      RowBox[{"-", "0.3808535485970228`"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "2"}], "]"}], "\[Rule]", 
      RowBox[{"-", "10.979481912107921`"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "3"}], "]"}], "\[Rule]", 
      RowBox[{"-", "0.19665480379455527`"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "4"}], "]"}], "\[Rule]", 
      RowBox[{"-", "0.42141500655251124`"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "5"}], "]"}], "\[Rule]", "0.6584527300961083`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "1"}], "]"}], "\[Rule]", 
      RowBox[{"-", "3.261238467195124`"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "2"}], "]"}], "\[Rule]", 
      RowBox[{"-", "2.0631556896285772`*^-8"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "3"}], "]"}], "\[Rule]", "5.819659430757391`"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "4"}], "]"}], "\[Rule]", "0.37746704753242283`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "5"}], "]"}], "\[Rule]", "3.023429578158533`"}]}], 
    "}"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.516708534127338*^9, 3.516708546737059*^9}, {
  3.5167085869383583`*^9, 3.516708631586912*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"A", "=", 
   RowBox[{"Table", "[", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"i", ",", "j"}], "]"}], ",", 
     RowBox[{"{", 
      RowBox[{"i", ",", "5"}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{"j", ",", "5"}], "}"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.516708550491274*^9, 3.516708565841152*^9}, {
  3.5219868472809534`*^9, 3.5219868535253105`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"A", "/.", "atest"}]], "Input",
 CellChangeTimes->{{3.5167085746056533`*^9, 3.5167085760477357`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{"1.19689396035823`", ",", "0", ",", "0", ",", "0", ",", "0"}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
    "0.20214668746590925`", ",", "7.840374588741148`", ",", 
     "0.22483245436904015`", ",", 
     RowBox[{"-", "0.8069757285872418`"}], ",", "0.18712786224886793`"}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "0.0828259043763153`"}], ",", "13.315981803707265`", ",", 
     "0.20729365656915424`", ",", "4.154819403051306`", ",", 
     "0.2573791697685864`"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "0.3808535485970228`"}], ",", 
     RowBox[{"-", "10.979481912107921`"}], ",", 
     RowBox[{"-", "0.19665480379455527`"}], ",", 
     RowBox[{"-", "0.42141500655251124`"}], ",", "0.6584527300961083`"}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "3.261238467195124`"}], ",", 
     RowBox[{"-", "2.0631556896285772`*^-8"}], ",", "5.819659430757391`", ",",
      "0.37746704753242283`", ",", "3.023429578158533`"}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVy19MkmEYhnFoyGw5IuZwzWy2ZuSkgUGtpIiXcCsQJ+YQWTUzW7ZWRLMQ
osnCVmrMrSip4Vc4U5bWDGhtRW5GHvRnRuaYtbY8wMIvcrTBhtRmz3Nw77f7
4NrUYqo/wWQwGIdg6nj+wh0JTbLM3gv94LvqoyIPaBLXx9Bxa0IxCA4+930Z
BR9JB6LndtDkw4BqnRmU6jbKUGXp15J28I8xt2IFn6Ye77SBAuFPFZqciPiI
jCYMV0MYdSStZW6Q09b1wriHJqIlIxUEh+Ufr4VAf+tNskVOk/vB3YUCkO2s
8mwFw8taTh3ozp8JHAaVxh9z6LT+8hEHKDx5Y2YEv9ewzQ86wi1OdGLSYsjf
R5Oubge3HOxpPhhHm45VRofA0qopwTA4e7ssM6ygyebTte+zYJ78JbUM5moq
tlsITTp+FU2jDWPmRSu4Mm4+ZQOpkZK9l0CnqliDRm219+xgZ+8aP/rGm3qG
8q8GNFkwlC6wo28lWeky9p/UTqWSJufNSQ96sY/XVQ3yYgsPUXWy5zVq6K48
qwFj+lE32lhTodWCeUtcAzrv4PehxbuOF9WCn9squI2gLbGoQweE6m+ofOjf
pAEMMcVzaEfmu74JXF2wytIOXm+Ob7CCuSsZEwVKWOnOW/tpkkglRW6w0Meu
Q9ezZ8+glDviQjmJiBdN21VjKC17NYWyZA9+o16xa6gfDCgV5UoVTe7yedQB
cO1fna0VfDIfZOXA/+zYQpI=
  "]]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"yhsolve", " ", "=", " ", 
   RowBox[{
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Flatten", "[", "yplus", "]"}], ",", 
      RowBox[{"{", "h", "}"}]}], "]"}], "/.", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "\[Rule]", " ", 
      RowBox[{"\[Beta]", "[", "t", "]"}]}], "}"}]}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"MatrixForm", "[", "yhsolve", "]"}]}], "Input",
 CellChangeTimes->{{3.5159453782986727`*^9, 3.515945470066921*^9}, {
   3.515946921330929*^9, 3.515946925987195*^9}, 3.5159579961003695`*^9}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", 
   TagBox[GridBox[{
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "5"}], "]"}], "-", 
        FractionBox[
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
            "]"}]}]}]]}]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"3", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"4", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"5", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]},
      {
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
           RowBox[{"514", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{"\[Beta]", "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
           RowBox[{"685", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{"\[Beta]", "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}]}], "250000"]}
     },
     GridBoxAlignment->{
      "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}},
        "RowsIndexed" -> {}},
     GridBoxSpacings->{"Columns" -> {
         Offset[0.27999999999999997`], {
          Offset[0.5599999999999999]}, 
         Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
         Offset[0.2], {
          Offset[0.4]}, 
         Offset[0.2]}, "RowsIndexed" -> {}}],
    Column], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->CompressedData["
1:eJwV0FtI02Ech/GVol4ssSHKShPt4Exj4WGQWvmakvlXZyPnaaGJRgVqibmm
koqhhGWRQ4fo8NwiQdAp4SlaJpFhIiYKE0vE8o11IM9k9f1dPHzuH++sfFXO
HpFIlI6yPSbWfQI5W/Z8nSGDIoO0xh8Gi7VVcijWtttC4N81r70K2Lb40bs+
iDPhRe3tBpjscEVugAEtA02k1VLeQhqsabPkkLuItcFnswPzz+Hmk/apmyGc
hZUk7b8FFUWyMNIWuuBZCNX8zz8dFM/0KIqhvZJHkfo8eRsL48y0mzRMbl0q
OaqH6uaqwbRwzobnNcY+2PtyqtoMH5jr2LEznI0uhbr6wsn4cIMMHq9ROidC
i+NMrwZ2zX+ZI7Oi7l4uh0dCa6efwpie9BMmqArKqSQPl+pSnM5y5pNQ4eIH
RyXCMtkvDZ7qgFcbx3274Cc33/WuCM54pnJiC2bGDRu3YftX/0Atw5cf0kmy
+n7Bqg5m1RdcL4ajs4dOl8AFlYdAqgaVjaXwg0VsIqOtv/pJhwqzsAUvLO0r
JZO8doK3YepBoTIykjMXhc1A3jnlei8aFg2tdJLej2ss5IGywDwBNsV160lj
d0B8POyolqSQNyxuj0injGz3BChd83dJhu+8vl0kc7djreTCtd1XKVDTfHKO
jG34rE6F9Zt22kKo1ax46KAwuJFvhD991svqznEm2bTJ9dDurWMiuTo9k0uO
N449JJ1HxprIuffR3aRlceQNudjXaiNl52s7GuD3jQi/yCjOJn5LjDHQFK4u
zoZxnWb7HfgfTWhjQA==
  "]]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"MatrixForm", "[", 
  RowBox[{"yhsolve", "\[LeftDoubleBracket]", 
   RowBox[{"{", 
    RowBox[{"2", ",", "3", ",", "1", ",", "5", ",", "4"}], "}"}], 
   "\[RightDoubleBracket]"}], "]"}]], "Input",
 CellChangeTimes->{{3.515892100226381*^9, 3.515892103605574*^9}, {
  3.515945478404398*^9, 3.5159455056989594`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", 
   TagBox[GridBox[{
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"3", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"4", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "5"}], "]"}], "-", 
        FractionBox[
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
            "]"}]}]}]]}]},
      {
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
           RowBox[{"514", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{"\[Beta]", "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
           RowBox[{"685", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{"\[Beta]", "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}]}], "250000"]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"5", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]}
     },
     GridBoxAlignment->{
      "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}},
        "RowsIndexed" -> {}},
     GridBoxSpacings->{"Columns" -> {
         Offset[0.27999999999999997`], {
          Offset[0.5599999999999999]}, 
         Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
         Offset[0.2], {
          Offset[0.4]}, 
         Offset[0.2]}, "RowsIndexed" -> {}}],
    Column], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{{3.5158920906568336`*^9, 3.515892103941593*^9}, 
   3.515894251870448*^9, 3.5158942865134296`*^9, 3.515894327309763*^9, 
   3.515894727905676*^9, 3.515895140546277*^9, {3.515895196225462*^9, 
   3.515895215001536*^9}, {3.5158958853858795`*^9, 3.515895898747644*^9}, 
   3.515896800193204*^9, 3.515897625598414*^9, 3.5159455061889873`*^9, 
   3.5159461116406174`*^9, 3.5159580155244803`*^9, 3.5159645082180004`*^9, 
   3.5159726597702427`*^9, 3.5159857731522846`*^9, 3.5159859387297554`*^9, 
   3.516707086759553*^9, 3.5167098045720034`*^9, 3.516711291119029*^9, 
   3.516712626927433*^9, 3.516731912075901*^9, 3.516751565777707*^9, 
   3.5176792371846943`*^9, 3.517679754760298*^9, 3.5176800893654366`*^9, 
   3.5176859990124483`*^9, 3.5176980479846106`*^9, 3.517698485554638*^9, 
   3.5176985994171505`*^9, 3.520728341318468*^9, 3.5207285467292166`*^9, 
   3.5208973238991246`*^9, 3.5213040202557087`*^9, 3.521486066962582*^9, 
   3.521487934570403*^9, 3.522245132659792*^9, 3.5222468178771806`*^9, 
   3.5222499219405003`*^9, 3.5223307385347404`*^9, 3.522356630688347*^9, 
   3.5223569353077183`*^9, 3.522429675974849*^9, 3.522510484503601*^9, 
   3.5225112109541516`*^9, 3.522511899235519*^9, 3.5227081638504066`*^9, 
   3.5227751720677896`*^9, 3.522776866401041*^9, 3.5230286097211437`*^9, 
   3.5230313582493505`*^9, 3.523557289268545*^9, 3.5237248905105815`*^9, 
   3.5237265819183245`*^9, 3.5239837613931494`*^9, 3.5239850088585*^9, 
   3.5239893236052895`*^9, 3.523990347607859*^9, 3.5239920409987154`*^9, 
   3.523992152918117*^9, 3.5239948345304966`*^9, 3.523994944437783*^9, 
   3.5239950555951405`*^9, 3.5242481576947527`*^9, 3.5242485444628744`*^9, 
   3.5242500238793287`*^9, 3.524400154356089*^9, 3.524400327461852*^9, 
   3.5244063046750183`*^9, 3.524406575240385*^9, 3.5244068044013443`*^9, 
   3.5244184176580157`*^9, 3.5244187254123535`*^9, 3.5244222494390736`*^9, 
   3.5244223366260605`*^9, 3.5244227767172327`*^9, 3.5244238671726027`*^9, 
   3.524442241392548*^9, 3.5244427673856335`*^9, 3.5244438743699493`*^9, 
   3.524445711944052*^9, 3.52444588953821*^9, 3.5244469031027207`*^9, 
   3.524502456182976*^9, 3.5245119752527905`*^9, 3.5245884157482433`*^9, 
   3.5250903036785154`*^9, 3.525091711469412*^9, 3.5250919682811675`*^9, 
   3.525092206673747*^9, 3.5250924621748953`*^9, 3.52509258217408*^9, 
   3.5250927062222576`*^9, 3.525092909963564*^9, 3.52509322909921*^9, 
   3.5250987884861884`*^9, 3.525448010053144*^9, 3.525461232625262*^9, 
   3.5255182907289066`*^9, 3.52582454967918*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"Theta5Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "2", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Theta2Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "3", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Theta1Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"ThetabetaCond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "5", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{"\[Beta]", "[", "t", "]"}]}], "]"}], "\[LeftDoubleBracket]", 
    "2", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Theta3Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "4", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{
          RowBox[{
           RowBox[{"Transpose", "[", 
            RowBox[{"{", 
             RowBox[{"{", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], ",", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], ",", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], ",", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                RowBox[{"\[Beta]", "[", "t", "]"}]}], ",", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], 
             "}"}], "]"}], "/.", "\[InvisibleSpace]", "Theta3Cond"}], "/.", 
          "\[InvisibleSpace]", "atest"}], "/.", "\[InvisibleSpace]", 
         "ThetabetaCond"}], "/.", "\[InvisibleSpace]", "atest"}], "/.", 
       "\[InvisibleSpace]", "Theta1Cond"}], "/.", "\[InvisibleSpace]", 
      "atest"}], "/.", "\[InvisibleSpace]", "Theta5Cond"}], "/.", 
    "\[InvisibleSpace]", "atest"}], "/.", "\[InvisibleSpace]", "Theta2Cond"}],
   "/.", "\[InvisibleSpace]", "atest"}]}], "Input",
 CellChangeTimes->{{3.51594676032572*^9, 3.5159468099305573`*^9}, {
   3.5159566741957607`*^9, 3.51595678713122*^9}, {3.5159568457875752`*^9, 
   3.5159568538670373`*^9}, {3.515956895125397*^9, 3.5159569150995398`*^9}, {
   3.5159571238684807`*^9, 3.515957170348139*^9}, {3.515958003729806*^9, 
   3.515958007675031*^9}, {3.5159638958349743`*^9, 3.515963896592017*^9}, {
   3.51596399752379*^9, 3.51596400259408*^9}, {3.515964082366643*^9, 
   3.515964102957821*^9}, {3.5159641483604174`*^9, 3.515964231857193*^9}, {
   3.515964279459916*^9, 3.5159642822650766`*^9}, {3.5159728721103873`*^9, 
   3.5159729359940414`*^9}, {3.515973121749666*^9, 3.5159731262559233`*^9}, 
   3.516711531312767*^9, {3.52072837720152*^9, 3.520728378225579*^9}, {
   3.522511106163158*^9, 3.5225111066501856`*^9}, {3.522511147690533*^9, 
   3.5225111497466507`*^9}, {3.5225118207090273`*^9, 3.522511820941041*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{"-", "2.6514877315488175`"}], "}"}], ",", 
   RowBox[{"{", "0.27759918149908547`", "}"}], ",", 
   RowBox[{"{", "2.136079661013141`", "}"}], ",", 
   RowBox[{"{", "0.8510021438341864`", "}"}], ",", 
   RowBox[{"{", "0.17455326539227106`", "}"}]}], "}"}]], "Output",
 CellChangeTimes->{{3.515964159175036*^9, 3.5159642329842577`*^9}, 
   3.5159642830411205`*^9, 3.5159726617303543`*^9, 3.5159727592559323`*^9, {
   3.5159728696132445`*^9, 3.5159729367200828`*^9}, {3.5159731227897253`*^9, 
   3.5159731277000065`*^9}, 3.5159734080060387`*^9, 3.5159737266572647`*^9, 
   3.5159857780885673`*^9, 3.515985944493085*^9, 3.5167070882696395`*^9, 
   3.516709806406108*^9, 3.5167112928461275`*^9, 3.5167122145968485`*^9, 
   3.5167126286265297`*^9, 3.5167319136439905`*^9, 3.5167515672867937`*^9, 
   3.5176792411009183`*^9, 3.5176797563663893`*^9, 3.517680091134537*^9, 
   3.5176860006535425`*^9, 3.517698049568701*^9, 3.517698487197732*^9, 
   3.5176986011342487`*^9, {3.520728361759637*^9, 3.520728379207635*^9}, 
   3.5207285477412744`*^9, 3.520897325058191*^9, 3.521304021449777*^9, 
   3.521486068197653*^9, 3.52148793555746*^9, 3.522245133550843*^9, 
   3.5222468187482305`*^9, 3.5222499231065674`*^9, 3.5223307403468437`*^9, 
   3.522356631655549*^9, 3.52235693622812*^9, 3.5224296770979137`*^9, 
   3.5225104858696795`*^9, 3.5225111085472946`*^9, {3.522511150420689*^9, 
   3.522511160592271*^9}, 3.5225112124152355`*^9, 3.5225119004915905`*^9, 
   3.522708165066476*^9, 3.522775172848834*^9, 3.5227768677071157`*^9, 
   3.523028610939213*^9, 3.523031359257408*^9, 3.5235572904366117`*^9, 
   3.5237248913266287`*^9, 3.5237265827213707`*^9, 3.5239837621691933`*^9, 
   3.523985009654546*^9, 3.523989324383334*^9, 3.5239903483939037`*^9, 
   3.5239920417877607`*^9, 3.5239921537041616`*^9, 3.523994835339543*^9, 
   3.5239949452308283`*^9, 3.523995056382186*^9, 3.5242481585117993`*^9, 
   3.5242485452519197`*^9, 3.5242500253394117`*^9, 3.5244001551201324`*^9, 
   3.5244003282268963`*^9, 3.5244063054780636`*^9, 3.5244065760114255`*^9, 
   3.5244068051893673`*^9, 3.524418418448017*^9, 3.524418726211396*^9, 
   3.52442225025712*^9, 3.5244223374451075`*^9, 3.524422777508278*^9, 
   3.524423867975649*^9, 3.5244422421665926`*^9, 3.5244427681956797`*^9, 
   3.5244438751769953`*^9, 3.5244457127200966`*^9, 3.5244458903142543`*^9, 
   3.524446904087777*^9, 3.524502457392045*^9, 3.5245119767248745`*^9, 
   3.5245884165652895`*^9, 3.52509030445756*^9, 3.5250917122594547`*^9, 
   3.5250919690652122`*^9, 3.5250922074457912`*^9, 3.5250924629539375`*^9, 
   3.525092582946124*^9, 3.5250927070153027`*^9, 3.5250929107445884`*^9, 
   3.52509323015527*^9, 3.525098789498246*^9, 3.5254480110241995`*^9, 
   3.525461233431308*^9, 3.5255182925120087`*^9, 3.525824550994255*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{"Theta1a", " ", "=", " ", 
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "/.", 
   "Theta1Cond"}]}], "\[IndentingNewLine]", 
 RowBox[{"Theta2a", " ", "=", " ", 
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "/.", 
   "Theta2Cond"}]}], "\[IndentingNewLine]", 
 RowBox[{"Theta3a", " ", "=", " ", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "/.", "Theta3Cond"}], "/.",
    "ThetabetaCond"}]}], "\[IndentingNewLine]", 
 RowBox[{"Theta4a", " ", "=", " ", 
  RowBox[{
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], " ", "-", 
     RowBox[{"\[Beta]", "[", "t", "]"}]}], "/.", "Theta3Cond"}], "/.", 
   "ThetabetaCond"}]}], "\[IndentingNewLine]", 
 RowBox[{"Theta5a", " ", "=", " ", 
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "/.", 
   "Theta5Cond"}]}]}], "Input",
 CellChangeTimes->{{3.515957319602676*^9, 3.515957408978788*^9}, {
   3.5159580291182575`*^9, 3.51595803894882*^9}, {3.515958695734386*^9, 
   3.5159587502905064`*^9}, 3.51595934042026*^9, {3.515963224972603*^9, 
   3.5159632256866436`*^9}, {3.515963471284691*^9, 3.515963496007105*^9}, {
   3.516712231378809*^9, 3.516712233642938*^9}}],

Cell[BoxData[
 RowBox[{"-", 
  RowBox[{"ArcCos", "[", 
   RowBox[{"-", 
    RowBox[{
     RowBox[{"(", 
      RowBox[{"685", "+", 
       RowBox[{"514", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "-", 
       RowBox[{"514", " ", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "1"}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "-", 
       RowBox[{"514", " ", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "5"}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}]}], ")"}], 
     "/", 
     RowBox[{"(", 
      RowBox[{"\[Sqrt]", 
       RowBox[{"(", 
        RowBox[{"469225", "+", 
         RowBox[{"469225", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "1"}], "]"}], "2"]}], "+", 
         RowBox[{"938450", " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "5"}], "]"}]}], "+", 
         RowBox[{"469225", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "5"}], "]"}], "2"]}], "+", 
         RowBox[{"704180", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"704180", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "1"}], "]"}], "2"], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"1408360", " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "5"}], "]"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"704180", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "5"}], "]"}], "2"], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"264196", " ", 
          SuperscriptBox[
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}], 
         "+", 
         RowBox[{"264196", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "1"}], "]"}], "2"], " ", 
          SuperscriptBox[
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}], 
         "+", 
         RowBox[{"528392", " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "5"}], "]"}], " ", 
          SuperscriptBox[
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}], 
         "+", 
         RowBox[{"264196", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "5"}], "]"}], "2"], " ", 
          SuperscriptBox[
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}], 
         "+", 
         RowBox[{"264196", " ", 
          SuperscriptBox[
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}], 
         "+", 
         RowBox[{"264196", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "1"}], "]"}], "2"], " ", 
          SuperscriptBox[
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}], 
         "+", 
         RowBox[{"528392", " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "5"}], "]"}], " ", 
          SuperscriptBox[
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}], 
         "+", 
         RowBox[{"264196", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "5"}], "]"}], "2"], " ", 
          SuperscriptBox[
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}]}], 
        ")"}]}], ")"}]}]}], "]"}]}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVzl1I02EYxuG5ipZpLhM3Bg2k/BhYLk1Lt7m9S0dszNaC3MKKxbQstyYM
bGviwCIiRoULV40Rtv1nZSAzPJAhOZgSmmhozQql0GBvhX2YbQXR8xzcXNxn
v4IzF/XNGSwW6yhMmKwN+Soomcx89BrdfVxJ+kC7LrXwBFS3yWdtlZQsj93f
3g4uVnRKUEmgQGgHsz/IWE7wxsrfKnSeVNejvz9PPCUSSvwrjijaW/qw0Av6
PJGRE1JKBqXXA0Pgv61r156BLfEhUlRLSdpozCsGR5uafSWgTeXZpkPT65Em
sHCKs4AmFMGTbpCd//xVGMy6emtPP2iduN2NXhKEDBw5JZk5fq4IzBc7ltGD
ueaZICgtocUMmPVW94tRUCIotUymwBZpIpAGVT9V5R2Ekmh11TT6WORJOkCF
29PqBJm2GtllUBo+oEG/xC7cc4Falagf7ZzLHkb5P2Y1KTBPKHKhn3h7K9Og
etHcrVRCp4lzF/UOiK/Ug6ePZTBo7qZwDB1bPWLVgOPKF160z9eg1YLtljID
OsPfdxP9vtbFawDj8ypuI6h3btaj9tGz79HEG17MAJ4fUSdQ1vqGRiP4LVne
YQddrfydDvzWHbYAyLzLcfccoqROsEXsBYtku3RolLtqQdl3ljxozf4lP6p4
aRpAh7sScfRUZPwrOjc9GOwFz02ZRMo6SuQPygKHQTbT4zSDodjHjX/A/5Y0
NGc=
  "]],

Cell[BoxData[
 RowBox[{
  RowBox[{"a", "[", 
   RowBox[{"4", ",", "1"}], "]"}], "+", 
  RowBox[{"a", "[", 
   RowBox[{"4", ",", "5"}], "]"}]}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVzl1I02EYxuG5ipZpLhM3Bg2k/BhYLk1Lt7m9S0dszNaC3MKKxbQstyYM
bGviwCIiRoULV40RNv+zMpAZHsiQHEwJTTS0ZoVSaLC3wj7MtgTpeQ5uLu6z
X965y/rGNBaLdRwmTFT3+soomUh/8hbde1JJekC7Ljn/DFS3yGds5ZQsjT7c
2QoulLVLUEkgT2gHMz/JWE7w1vJ6BTpHKmvRv1/HnxMJJf5lRwTtLn6c7wV9
nvDwKSklA9KbgUFwY/vqjRdgU2yQFFRTkjIacwrBkYZGXxFoU3l26NDUWrgB
zJ/kzKNxRfC0G2TnvnwTAjOu39nXB1rH73aiVwS9Bo6ckvQsP1cE5oodS+jh
bPN0EJQW0UIGzHiv+8MoKBEUWyaSYJM0HkiBqt+q0jZCSaSyYgp9KvIkHKDC
7Wl2gkxLlewqKA0d0qDfopceuECtStSHts9mDqH8XzOaJJgjFLnQL7z95SlQ
vWDuVCqh08S5j3r7xddqwbMn0hg0e0soio6uHLNqwDHlKy/a46vTasFWS4kB
neYfuI3+XO3g1YGxORW3HtQ7t+pR+8j5j2j8HS9qAC8Oq+Moa21TvRH8kSht
s4OuZv5uB37rLlsAZD5kubuOUFIj2Cb2ggWyPTo0wl2xoOx7ix606uCiH1W8
NvWjQx3xGHomPPYdnZ0aCHaDFyZNImUNJfJHJYGjIJvpcprBjfXPm/+B/wGa
izT5
  "]],

Cell[BoxData[
 RowBox[{
  RowBox[{"a", "[", 
   RowBox[{"5", ",", "1"}], "]"}], "+", 
  RowBox[{"a", "[", 
   RowBox[{"5", ",", "5"}], "]"}], "-", 
  RowBox[{
   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "-", 
  RowBox[{
   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVzl1I02EYxuG5ipbNXCZuDBpI+TGwXJqWbnN7l45wzNaC3MI+FtOy3Jow
sK2JA4uIGBUuXDWG2PzPykBmeCBDcjAlNNHQmhVKpMHeCvsw2wqi5zm4ubjP
frlnL+kb01gs1lGYKFHV5yulZDL90Wt093EV6QXtuuTCE7C2RTFrK6NkeezB
9lZwsbRdikoDuSI7mPFBznKCN1f+lqPzpKIG/f154imRUuJfcUTQ7qKHeV7Q
5wmPnJBRMii7ERgC/21du/4MbIoNkfwqSlJGY3YBONrQ6CsEbWrPNh2aWg83
gHlTnAU0rgyedIPsnOevQiD32u09/aB14k4nelnYZ+AoKEnP9PPEYI7EsYwe
zDLPBEFZIS1gQO5b3S9GSYmwyDKZBJtk8UAKVP9Ul7QRSiIV5dPoY7En4QCV
bk+zE2RaKuVXQFnogAb9Er143wVq1eJ+tH0uYxgV/JjVJMFskdiFfuLvLUuB
tYvmTpUKOk2ce6h3QHK1Bjx9LI1BszaFoujY6hGrBhxXvfCivb46rRZstRQb
0BnBvlvo97UOfh0Ym1fz6kG9c7MetY+ee4/G3/CjBvDCSG0cZa1vqDeC3xIl
bXbQ1SzY6cBv3WELgMy7THfXIUqqhVskXjBfvkuHRnirFpR9d8mDVu5f8qPK
l6YBdLgjHkNPhce/onPTg8Fu8PyUSayqpkTRUxw4DLKZLqcZPMP9uPEP+B+P
GDNt
  "]],

Cell[BoxData[
 RowBox[{
  RowBox[{"a", "[", 
   RowBox[{"5", ",", "1"}], "]"}], "+", 
  RowBox[{"a", "[", 
   RowBox[{"5", ",", "5"}], "]"}], "-", 
  RowBox[{"ArcCos", "[", 
   RowBox[{
    RowBox[{"(", 
     RowBox[{"528392", "+", 
      RowBox[{"704180", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "+", 
      RowBox[{"704180", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{"2", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{"528392", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{"2", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
         RowBox[{"2", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], "]"}]}], "+", 
      RowBox[{"938450", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{"938450", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{"2", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{"704180", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{"2", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
         RowBox[{"2", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{"704180", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}], "-", 
      RowBox[{"\[Sqrt]", 
       RowBox[{"(", 
        RowBox[{
         SuperscriptBox[
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", "528392"}], "-", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "-", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], 
            "-", 
            RowBox[{"528392", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], "]"}]}], 
            "-", 
            RowBox[{"938450", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], 
            "-", 
            RowBox[{"938450", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], 
            "-", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], 
            "-", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
           ")"}], "2"], "-", 
         RowBox[{"4", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"469225", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{"2", " ", 
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "]"}]}], 
            "+", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "+", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], 
            "+", 
            RowBox[{"528392", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], "]"}]}], 
            "+", 
            RowBox[{"469225", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "-", 
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}]}], "]"}]}], 
            "+", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], 
            "-", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
           ")"}], " ", 
          RowBox[{"(", 
           RowBox[{"1466842", "+", 
            RowBox[{"1408360", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
           ")"}]}]}], ")"}]}]}], ")"}], "/", 
    RowBox[{"(", 
     RowBox[{"2", " ", 
      RowBox[{"(", 
       RowBox[{"1466842", "+", 
        RowBox[{"1408360", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], ")"}]}],
      ")"}]}], "]"}], "-", 
  RowBox[{
   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "-", 
  RowBox[{
   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVzl1I02EYxuG5ipZpLhM3Rg2k/BhYLk1Lt7m9S0dszNYC3cKKxbQstyYM
bGviwCIiRoULV40RNv+zMpAZHsiQHEwJTTS0ZoUSabC3wj7MtoLoeQ5uLu6z
X96Zi/qmNBaLdQwmTFT3+coomUx/9BrdU68kvaBdl1x4Aqpb5bO2ckqWx+5v
bwMXyzokqCSQJ7SDmR9kLCd4Y+VvBTpPKmvR358nnhIJJf4VRwTtKX6Y7wV9
nvDICSklg9LrgSHw39a1a8/A5tgQKaimJGU05hSCo41NviLQpvJs06Gp9XAj
mD/FWUDjiuBJN8jOff4qBGZcvbW3H7RO3O5CLwn6DBw5JelZfq4IzBU7ltFD
2eaZICgtooUMmPFW94tRUCIotkwmwWZpPJACVT9Vpe2EkkhlxTT6WORJOECF
29PiBJnWKtllUBo6qEG/RC/cc4Falagf7ZjLHEb5P2Y1STBHKHKhn3j7ylOg
etHcpVRCp4lzF/UOiK/UgqePpzFo9qZQFB1bPWrVgOPKF16011en1YJtlhID
OsPffxP9vtbJqwNj8ypuA6h3btaj9tGz79H4G17UAJ4fUcdR1vqGBiP4LVHa
bgddLfxdDvzWHbYAyLzLcncfpqRGsEXsBQtku3VohLtqQdl3ljxo1YElP6p4
aRpAhzvjMfRUePwrOjc9GOwBz02ZRMoaSuQPSgJHQDbT7TSDO+s/bvwD/geO
/jNz
  "]],

Cell[BoxData[
 RowBox[{
  RowBox[{"a", "[", 
   RowBox[{"3", ",", "1"}], "]"}], "+", 
  RowBox[{"a", "[", 
   RowBox[{"3", ",", "5"}], "]"}]}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVzl1I02EYxuG5ipZpLhM3Bg2k/BhYLk1Lt7m9a45wzNaC3MKKxbQstyYM
bGviwEIiRoULV40ROv+zMpAZHsiQHEwJTTS0ZoUSabC3wj7MtoLoeQ5uLu6z
X965y/rGNBaLdRwmTFT3+8oomUp/9Brde1JJekG7Lrn4BKxtkc/ZyilZGX+w
sxVcKmuXoJJAntAOZn6QsZzgzdW/FegCqaxBf3+efEoklPhXHRG0p7gv3wv6
POHRU1JKhqQ3AsPgv+3rXc/AptgwKaimJGU05hSCYw2NviLQpvbs0KGpjXAD
mD/NWUTjiuBpN8jOff4qBGZcv71vALRO3ulErwj6DRw5JelZfq4IzBU7VtDD
2ebZICgtooUMmPFW94tRUCIotkwlwSZpPJAC1T/VpW2EkkhlxQz6WORJOECF
29PsBJmWKtlVUBo6pEG/RC/dd4FatWgAbZ/PHEH5P+Y0STBHKHKhn3j7y1Ng
7ZK5U6mEThPnHuodFF+rAc+eSGPQ7C2hKDq+dsyqASeUL7xor69OqwVbLSUG
dJZ/4Bb6fb2DVwfGFtTcelDv3KpH7WPn36PxN7yoAbw4WhtHWRub6o3gt0Rp
mx10NfN3O/Bbd9kCIPMuy919hBKVYJvYCxbI9ujQCHfNgrLvLnvQqoPLflTx
0jSIjnTEY+iZ8MRXdH5mKNgDXpg2iZQqSuQPSwJHQTbT7TSDqq6Pm/+A/wGR
lTPM
  "]]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a1\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta1a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, 3.5167122415113883`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a2\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta2a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, {3.5167122415113883`*^9, 3.516712243727515*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a3\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta3a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, {3.5167122415113883`*^9, 3.516712245702628*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a4\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta4a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, {3.5167122415113883`*^9, 3.5167122473267207`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a5\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta5a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, {3.5167122415113883`*^9, 3.5167122473267207`*^9}}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Velocity of Outputs: Pre and Post Impact", "Subsection",
 CellChangeTimes->{{3.5133462255725746`*^9, 3.5133462845249467`*^9}, {
  3.5140475842599964`*^9, 3.514047587214165*^9}, {3.514050381757004*^9, 
  3.5140503867632904`*^9}, {3.5140508345048995`*^9, 3.514050836366006*^9}, {
  3.515340123141806*^9, 3.515340124962953*^9}}],

Cell["compute the jacobian of the output functions", "Text",
 CellChangeTimes->{{3.514056134624614*^9, 3.514056152522016*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"Y", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"{", 
       SubscriptBox["p", "hip"], "}"}]}], ",", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", "2"], "]"}]}], "]"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5158871007064247`*^9, 3.515887101970497*^9}, 
   3.5158874557787333`*^9, {3.515898875609911*^9, 3.5158988760079336`*^9}, {
   3.5158993719242983`*^9, 3.5158993862991204`*^9}, {3.515899472917075*^9, 
   3.5158994757032337`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Yplus", "=", 
  RowBox[{
   RowBox[{
    RowBox[{"Y", "/.", "initialtime"}], "/.", "constsubs"}], "/.", 
   "resetmap"}]}]], "Input",
 CellChangeTimes->{{3.5158905547469845`*^9, 3.5158905560910616`*^9}, 
   3.515891153092208*^9, {3.515893378403488*^9, 3.5158933836337876`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{
       FractionBox["1", "250000"], 
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}]}]}]}], ",", 
     RowBox[{"-", 
      FractionBox[
       RowBox[{"32639", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], 
       "125000"]}], ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{"-", "1"}], "+", 
      RowBox[{
       FractionBox["1", 
        RowBox[{"250000", " ", 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]}]], 
       RowBox[{"127", " ", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "2"}], "]"}], " ", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "3"}], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}]}]}], "-", 
      RowBox[{
       FractionBox["1", 
        RowBox[{"250000", " ", 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]}]], 
       RowBox[{"127", " ", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "1"}], "]"}], " ", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "4"}], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}]}]}], "-", 
      FractionBox[
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "2"], 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "2"]]}], ",", 
     RowBox[{
      RowBox[{"-", 
       FractionBox[
        RowBox[{"733421", "+", 
         RowBox[{"704180", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}]}], 
        SuperscriptBox[
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}], "2"]]}], "+", 
      FractionBox[
       RowBox[{"32639", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "2"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "3"}], "]"}]}], "-", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "1"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "4"}], "]"}]}]}], ")"}], " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], 
       RowBox[{"125000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]]}], ",", 
     RowBox[{"-", 
      FractionBox[
       RowBox[{"733421", "+", 
        RowBox[{"704180", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}]}], 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "2"]]}], ",", 
     FractionBox[
      RowBox[{"733421", "+", 
       RowBox[{"704180", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}]}], 
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         RowBox[{"685", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}], "2"]], ",", 
     FractionBox[
      RowBox[{"685", " ", 
       RowBox[{"(", 
        RowBox[{"685", "+", 
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}]}], 
        ")"}]}], 
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         RowBox[{"685", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}], "2"]]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      FractionBox["1", 
       RowBox[{"250000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "2"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "3"}], "]"}]}], "-", 
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"685", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}]}]}], ",", 
     RowBox[{
      FractionBox["1", 
       RowBox[{"125000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"125000", " ", 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]}], "+", 
        RowBox[{"32639", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "3"}], "]"}]}], "-", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "1"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}], ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      FractionBox["1", 
       RowBox[{"250000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"4", ",", "2"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"4", ",", "3"}], "]"}]}], "-", 
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"4", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"685", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}]}]}], ",", 
     FractionBox[
      RowBox[{"32639", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"4", ",", "2"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"4", ",", "3"}], "]"}]}], "-", 
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"4", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}], " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], 
      RowBox[{"125000", " ", 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]}]], ",", "0", ",", "0", ",", "1"}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      FractionBox["1", 
       RowBox[{"250000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"250000", " ", 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]}], "+", 
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "3"}], "]"}]}], "-", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "1"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"514", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
           RowBox[{"685", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}]}]}], ")"}]}], ",", 
     RowBox[{
      FractionBox["1", 
       RowBox[{"125000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"125000", " ", 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]}], "+", 
        RowBox[{"32639", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "3"}], "]"}]}], "-", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "1"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}], ",", "1", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output",
 CellChangeTimes->{{3.5158905344828253`*^9, 3.515890556439081*^9}, 
   3.515891153534233*^9, 3.515893390296169*^9, 3.515894253081517*^9, 
   3.515894287868507*^9, 3.5158943286168375`*^9, 3.5158944883609743`*^9, 
   3.5158947301438036`*^9, 3.515895072985413*^9, 3.5158951418933544`*^9, {
   3.515895197464533*^9, 3.5158952162706084`*^9}, {3.5158958866249504`*^9, 
   3.5158959002417293`*^9}, 3.5158968023103247`*^9, 3.515897626941491*^9, 
   3.5158989571115723`*^9, 3.5158994278394966`*^9, 3.515899515927535*^9, 
   3.515946252155654*^9, 3.5159859215107703`*^9, 3.5167072878040524`*^9, 
   3.516710046536843*^9, 3.5167114897493896`*^9, 3.516712848420101*^9, 
   3.5167321044569044`*^9, 3.516751762734973*^9, 3.5176794457626243`*^9, 
   3.5176799539296894`*^9, 3.5176802905729446`*^9, 3.5176862026600966`*^9, 
   3.517698252724321*^9, 3.5176988064889946`*^9, 3.5207285490463495`*^9, 
   3.520897326494273*^9, 3.5213040228218555`*^9, 3.521486069622734*^9, 
   3.521487936879535*^9, 3.5222451348049145`*^9, 3.522246820785347*^9, 
   3.5222499245586505`*^9, 3.5223307417639256`*^9, 3.522356633090751*^9, 
   3.5223569375853224`*^9, 3.522429678518995*^9, 3.522510642068613*^9, 
   3.5225114181710033`*^9, 3.5225119023436966`*^9, 3.5227081669475837`*^9, 
   3.5227751742159123`*^9, 3.522776869801235*^9, 3.523028613269346*^9, 
   3.5230313611045136`*^9, 3.523557292282717*^9, 3.52372489275171*^9, 
   3.523726584098449*^9, 3.5239837636552787`*^9, 3.5239850110216236`*^9, 
   3.5239893257304115`*^9, 3.5239903497799835`*^9, 3.5239920431638393`*^9, 
   3.5239921550642395`*^9, 3.52399483669162*^9, 3.5239949466559095`*^9, 
   3.5239950577932663`*^9, 3.5242481599948845`*^9, 3.5242485465949965`*^9, 
   3.5242500282145767`*^9, 3.524400156416171*^9, 3.5244003295789733`*^9, 
   3.5244063069041433`*^9, 3.524406577338478*^9, 3.5244068065384407`*^9, 
   3.524418419818019*^9, 3.5244187276244497`*^9, 3.5244222518602123`*^9, 
   3.5244223390021963`*^9, 3.5244227788303533`*^9, 3.524423869357728*^9, 
   3.524442243543671*^9, 3.524442769556757*^9, 3.524443876589076*^9, 
   3.5244457140631733`*^9, 3.524445891658331*^9, 3.5244469058788795`*^9, 
   3.5245024595261674`*^9, 3.5245119777739344`*^9, 3.524588417214327*^9, 
   3.5250903050915966`*^9, 3.525091712831485*^9, 3.525091969652233*^9, 
   3.5250922080278244`*^9, 3.5250924635379705`*^9, 3.5250925835221567`*^9, 
   3.5250927076233377`*^9, 3.5250929113306217`*^9, 3.525093230970317*^9, 
   3.52509879026329*^9, 3.525448011766242*^9, 3.5254612340513434`*^9, 
   3.5255182935370674`*^9, 3.5258246030142307`*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<H_plus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"Yplus", "/.", "statesubs"}], "/.", "constsubs"}]}], "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158948955412636`*^9, 3.5158949312793083`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Yminus", "=", 
  RowBox[{
   RowBox[{"Y", "/.", "constsubs"}], "/.", "resetmap"}]}]], "Input",
 CellChangeTimes->{{3.5158912072393045`*^9, 3.515891227677474*^9}, {
  3.5158934248751464`*^9, 3.515893427941322*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}]}], "250000"]}], ",", 
     RowBox[{"-", 
      FractionBox[
       RowBox[{"32639", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], 
       "125000"]}], ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{"-", "1"}], "+", 
      RowBox[{
       FractionBox["1", 
        RowBox[{"250000", " ", 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]}]], 
       RowBox[{"127", " ", 
        SuperscriptBox["\[ExponentialE]", 
         RowBox[{"-", 
          FractionBox[
           RowBox[{"127", " ", 
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"-", "685"}], " ", 
               RowBox[{"Sin", "[", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
              RowBox[{"514", " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                  "+", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                 ")"}]}], "+", 
              RowBox[{"685", " ", 
               RowBox[{"Sin", "[", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}],
              ")"}]}], 
           RowBox[{"250000", " ", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "2"}], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "3"}], "]"}], " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{"1", "/", 
              RowBox[{"(", 
               RowBox[{"250000", " ", 
                RowBox[{"a", "[", 
                 RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
             RowBox[{"127", " ", 
              RowBox[{"a", "[", 
               RowBox[{"2", ",", "2"}], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"-", "685"}], " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"-", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                    "+", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   ")"}]}], "+", 
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}]}], "]"}]}], "-", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "1"}], "]"}], " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{"1", "/", 
              RowBox[{"(", 
               RowBox[{"250000", " ", 
                RowBox[{"a", "[", 
                 RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
             RowBox[{"127", " ", 
              RowBox[{"a", "[", 
               RowBox[{"2", ",", "2"}], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"-", "685"}], " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"-", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                    "+", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   ")"}]}], "+", 
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}]}], "-", 
      RowBox[{
       FractionBox["1", 
        RowBox[{"250000", " ", 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]}]], 
       RowBox[{"127", " ", 
        SuperscriptBox["\[ExponentialE]", 
         RowBox[{"-", 
          FractionBox[
           RowBox[{"127", " ", 
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"-", "685"}], " ", 
               RowBox[{"Sin", "[", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
              RowBox[{"514", " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                  "+", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                 ")"}]}], "+", 
              RowBox[{"685", " ", 
               RowBox[{"Sin", "[", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}],
              ")"}]}], 
           RowBox[{"250000", " ", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "4"}], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "1"}], "]"}], " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{"1", "/", 
              RowBox[{"(", 
               RowBox[{"250000", " ", 
                RowBox[{"a", "[", 
                 RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
             RowBox[{"127", " ", 
              RowBox[{"a", "[", 
               RowBox[{"2", ",", "2"}], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"-", "685"}], " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"-", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                    "+", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   ")"}]}], "+", 
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}]}], "]"}]}], "+", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "3"}], "]"}], " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{"1", "/", 
              RowBox[{"(", 
               RowBox[{"250000", " ", 
                RowBox[{"a", "[", 
                 RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
             RowBox[{"127", " ", 
              RowBox[{"a", "[", 
               RowBox[{"2", ",", "2"}], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"-", "685"}], " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"-", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                    "+", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   ")"}]}], "+", 
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}]}], "-", 
      RowBox[{
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "2"], "/", 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "2"]}]}], ",", 
     RowBox[{
      RowBox[{
       FractionBox["1", 
        RowBox[{"125000", " ", 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]}]], 
       RowBox[{"32639", " ", 
        SuperscriptBox["\[ExponentialE]", 
         RowBox[{"-", 
          FractionBox[
           RowBox[{"127", " ", 
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"-", "685"}], " ", 
               RowBox[{"Sin", "[", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
              RowBox[{"514", " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                  "+", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                 ")"}]}], "+", 
              RowBox[{"685", " ", 
               RowBox[{"Sin", "[", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}],
              ")"}]}], 
           RowBox[{"250000", " ", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "2"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "3"}], "]"}]}], "-", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "1"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "4"}], "]"}]}]}], ")"}], " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"1", "/", 
           RowBox[{"(", 
            RowBox[{"250000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "2"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "685"}], " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
             
             RowBox[{"514", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"-", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], 
               ")"}]}], "+", 
             RowBox[{"685", " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
            ")"}]}]}], "]"}], " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], "-", 
      RowBox[{
       RowBox[{"(", 
        RowBox[{"733421", "+", 
         RowBox[{"704180", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], ")"}], 
       "/", 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "2"]}], "-", 
      RowBox[{
       FractionBox["1", 
        RowBox[{"125000", " ", 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]}]], 
       RowBox[{"32639", " ", 
        SuperscriptBox["\[ExponentialE]", 
         RowBox[{"-", 
          FractionBox[
           RowBox[{"127", " ", 
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"-", "685"}], " ", 
               RowBox[{"Sin", "[", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
              RowBox[{"514", " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                  "+", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                 ")"}]}], "+", 
              RowBox[{"685", " ", 
               RowBox[{"Sin", "[", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}],
              ")"}]}], 
           RowBox[{"250000", " ", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "1"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "2"}], "]"}]}], "+", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "3"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "4"}], "]"}]}]}], ")"}], " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{"1", "/", 
           RowBox[{"(", 
            RowBox[{"250000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "2"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "685"}], " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
             
             RowBox[{"514", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"-", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], 
               ")"}]}], "+", 
             RowBox[{"685", " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
            ")"}]}]}], "]"}]}]}]}], ",", 
     RowBox[{"-", 
      RowBox[{
       RowBox[{"(", 
        RowBox[{"733421", "+", 
         RowBox[{"704180", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], ")"}], 
       "/", 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}], "2"]}]}], ",", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{"733421", "+", 
        RowBox[{"704180", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], ")"}], 
      "/", 
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"685", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}], "2"]}], ",", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{"685", " ", 
        RowBox[{"(", 
         RowBox[{"685", "+", 
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
         ")"}]}], ")"}], "/", 
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"685", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}], "2"]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      FractionBox["1", 
       RowBox[{"250000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{"127", " ", 
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"3", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "685"}], " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
             
             RowBox[{"514", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"-", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], 
               ")"}]}], "+", 
             RowBox[{"685", " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
            ")"}]}], 
          RowBox[{"250000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"685", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{"1", "/", 
             RowBox[{"(", 
              RowBox[{"250000", " ", 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}]}], "]"}]}], "-", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{"1", "/", 
             RowBox[{"(", 
              RowBox[{"250000", " ", 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}]}], ",", 
     RowBox[{
      FractionBox["1", 
       RowBox[{"125000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"3", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "685"}], " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
             
             RowBox[{"514", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"-", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], 
               ")"}]}], "+", 
             RowBox[{"685", " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
            ")"}]}], 
          RowBox[{"250000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"125000", " ", 
          SuperscriptBox["\[ExponentialE]", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "4"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}], 
            RowBox[{"250000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]]], " ", 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]}], "+", 
         RowBox[{"32639", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{"1", "/", 
             RowBox[{"(", 
              RowBox[{"250000", " ", 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}]}], "]"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
         RowBox[{"32639", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{"1", "/", 
             RowBox[{"(", 
              RowBox[{"250000", " ", 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}]}], ",", "0", ",", 
     "0", ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      FractionBox["1", 
       RowBox[{"250000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{"127", " ", 
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"4", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "685"}], " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
             
             RowBox[{"514", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"-", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], 
               ")"}]}], "+", 
             RowBox[{"685", " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
            ")"}]}], 
          RowBox[{"250000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"685", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"514", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{"1", "/", 
             RowBox[{"(", 
              RowBox[{"250000", " ", 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}]}], "]"}]}], "-", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{"1", "/", 
             RowBox[{"(", 
              RowBox[{"250000", " ", 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}]}], ",", 
     RowBox[{
      FractionBox["1", 
       RowBox[{"125000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{"32639", " ", 
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"4", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "685"}], " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
             
             RowBox[{"514", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"-", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], 
               ")"}]}], "+", 
             RowBox[{"685", " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
            ")"}]}], 
          RowBox[{"250000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{"1", "/", 
             RowBox[{"(", 
              RowBox[{"250000", " ", 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}]}], "]"}]}], "-", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{"1", "/", 
             RowBox[{"(", 
              RowBox[{"250000", " ", 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}]}], ",", "0", ",", 
     "0", ",", "1"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      FractionBox["1", 
       RowBox[{"250000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"5", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "685"}], " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
             
             RowBox[{"514", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"-", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], 
               ")"}]}], "+", 
             RowBox[{"685", " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
            ")"}]}], 
          RowBox[{"250000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"250000", " ", 
          SuperscriptBox["\[ExponentialE]", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "4"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}], 
            RowBox[{"250000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]]], " ", 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]}], "+", 
         RowBox[{"127", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{"1", "/", 
             RowBox[{"(", 
              RowBox[{"250000", " ", 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}]}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"685", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
            RowBox[{"514", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
           ")"}]}], "-", 
         RowBox[{"86995", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{"1", "/", 
             RowBox[{"(", 
              RowBox[{"250000", " ", 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}]}], "]"}]}], "-", 
         RowBox[{"65278", " ", 
          RowBox[{"a", "[", 
           RowBox[{"5", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"5", ",", "2"}], "]"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{"1", "/", 
             RowBox[{"(", 
              RowBox[{"250000", " ", 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}]}], "]"}]}], "-", 
         RowBox[{"65278", " ", 
          RowBox[{"a", "[", 
           RowBox[{"5", ",", "3"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"5", ",", "4"}], "]"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{"1", "/", 
             RowBox[{"(", 
              RowBox[{"250000", " ", 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}]}], ",", 
     RowBox[{
      FractionBox["1", 
       RowBox[{"125000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"5", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "685"}], " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
             
             RowBox[{"514", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"-", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], 
               ")"}]}], "+", 
             RowBox[{"685", " ", 
              RowBox[{"Sin", "[", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
            ")"}]}], 
          RowBox[{"250000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"125000", " ", 
          SuperscriptBox["\[ExponentialE]", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "4"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}], 
            RowBox[{"250000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]]], " ", 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]}], "+", 
         RowBox[{"32639", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{"1", "/", 
             RowBox[{"(", 
              RowBox[{"250000", " ", 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}]}], "]"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
         RowBox[{"32639", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{"1", "/", 
             RowBox[{"(", 
              RowBox[{"250000", " ", 
               RowBox[{"a", "[", 
                RowBox[{"1", ",", "1"}], "]"}]}], ")"}]}], 
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "685"}], " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
               "+", 
               RowBox[{"514", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"-", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                  ")"}]}], "+", 
               RowBox[{"685", " ", 
                RowBox[{"Sin", "[", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                 "]"}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}]}], ",", "1", ",", 
     "0", ",", "0"}], "}"}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.5158934289843817`*^9, 3.5158942531055183`*^9, 3.515894287910509*^9, 
   3.5158943286718407`*^9, 3.5158947302528095`*^9, 3.515895081600906*^9, 
   3.515895141928356*^9, {3.5158951974925346`*^9, 3.5158952163226113`*^9}, {
   3.5158958866509523`*^9, 3.5158959002927322`*^9}, 3.5158968023523273`*^9, 
   3.5158976269754925`*^9, 3.5158989644739933`*^9, 3.5158994374470463`*^9, 
   3.51589951601554*^9, 3.51594625225766*^9, 3.5159859216307774`*^9, 
   3.516707287884057*^9, 3.516710046647849*^9, 3.5167114898293943`*^9, 
   3.516712848489105*^9, 3.5167321045409093`*^9, 3.516751762833978*^9, 
   3.5176794458406286`*^9, 3.517679954009694*^9, 3.51768029066395*^9, 
   3.517686202745102*^9, 3.5176982528153257`*^9, 3.517698806587*^9, 
   3.5207285490913515`*^9, 3.520897326565277*^9, 3.521304022881859*^9, 
   3.521486069702739*^9, 3.5214879369425387`*^9, 3.522245134848917*^9, 
   3.5222468208523507`*^9, 3.522249924636655*^9, 3.522330741814928*^9, 
   3.522356633184352*^9, 3.5223569376477222`*^9, 3.5224296785479965`*^9, 
   3.5225106421386175`*^9, 3.52251141828201*^9, 3.522511902420701*^9, 
   3.5227081670145874`*^9, 3.5227751742889166`*^9, 3.5227768698592386`*^9, 
   3.5230286133693523`*^9, 3.523031361161517*^9, 3.523557292343721*^9, 
   3.523724892801713*^9, 3.523726584158453*^9, 3.52398376368328*^9, 
   3.5239850110806274`*^9, 3.5239893257684135`*^9, 3.523990349818986*^9, 
   3.5239920432188425`*^9, 3.523992155105242*^9, 3.5239948367506237`*^9, 
   3.5239949467229137`*^9, 3.5239950578332686`*^9, 3.524248160026886*^9, 
   3.524248546633999*^9, 3.5242500283945866`*^9, 3.5244001564461713`*^9, 
   3.5244003296139755`*^9, 3.5244063069421453`*^9, 3.5244065773754797`*^9, 
   3.524406806574443*^9, 3.524418419898019*^9, 3.52441872767245*^9, 
   3.524422251910215*^9, 3.5244223390812006`*^9, 3.5244227788563547`*^9, 
   3.5244238694027305`*^9, 3.524442243571673*^9, 3.52444276960376*^9, 
   3.524443876625078*^9, 3.5244457140971756`*^9, 3.524445891687333*^9, 
   3.5244469059338827`*^9, 3.524502459590171*^9, 3.524511977830938*^9, 
   3.5245884172513285`*^9, 3.525090305133599*^9, 3.5250917128714848`*^9, 
   3.5250919697002363`*^9, 3.5250922080618267`*^9, 3.525092463576973*^9, 
   3.525092583559159*^9, 3.5250927076743402`*^9, 3.5250929113916254`*^9, 
   3.525093231041321*^9, 3.5250987903072925`*^9, 3.525448011816245*^9, 
   3.5254612340933456`*^9, 3.525518293683076*^9, 3.525824603075234*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<H_minus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Yminus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.515895085079105*^9, 3.5158950853751216`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{"h", "\[IndentingNewLine]", 
 RowBox[{"Dh", " ", "=", " ", 
  RowBox[{
   SubscriptBox["\[PartialD]", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
   RowBox[{"{", "h", "}"}]}]}]}], "Input",
 CellChangeTimes->{{3.5158876222012525`*^9, 3.5158876466726522`*^9}, {
  3.515895637521703*^9, 3.5158956394738145`*^9}}],

Cell[BoxData[
 RowBox[{
  FractionBox["1", "250000"], 
  RowBox[{"127", " ", 
   RowBox[{"(", 
    RowBox[{
     RowBox[{"685", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
     RowBox[{"514", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
     RowBox[{"514", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
     RowBox[{"685", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
    ")"}]}]}]], "Output",
 CellChangeTimes->{{3.5158956386727686`*^9, 3.5158956398638363`*^9}, {
   3.5158958866799536`*^9, 3.515895900344735*^9}, 3.515896802422331*^9, 
   3.515897627033496*^9, 3.5159462523356647`*^9, 3.515985921767785*^9, 
   3.516707287973062*^9, 3.5167100467628555`*^9, 3.5167114899284*^9, 
   3.516712848515107*^9, 3.5167321046199136`*^9, 3.5167517629159827`*^9, 
   3.517679445919633*^9, 3.5176799540956993`*^9, 3.517680290765956*^9, 
   3.517686202845107*^9, 3.517698252865329*^9, 3.517698806692006*^9, 
   3.520728549140354*^9, 3.5208973266942844`*^9, 3.521304022935862*^9, 
   3.521486069771743*^9, 3.521487936998542*^9, 3.52224513490092*^9, 
   3.522246821400382*^9, 3.5222499247016582`*^9, 3.522330741871931*^9, 
   3.5223566332623515`*^9, 3.522356937710122*^9, 3.5224296785949993`*^9, 
   3.5225106422336226`*^9, 3.5225114183960166`*^9, 3.522511902505706*^9, 
   3.522708167077591*^9, 3.52277517434692*^9, 3.5227768699202423`*^9, 
   3.523028613473358*^9, 3.5230313612175198`*^9, 3.523557292400724*^9, 
   3.5237248928757167`*^9, 3.5237265842264566`*^9, 3.5239837637362833`*^9, 
   3.523985011164632*^9, 3.523989325828417*^9, 3.523990349878989*^9, 
   3.523992043299847*^9, 3.5239921551712456`*^9, 3.523994836828628*^9, 
   3.523994946803918*^9, 3.523995057892272*^9, 3.524248160081889*^9, 
   3.5242485466930017`*^9, 3.5242500287246056`*^9, 3.5244001564861712`*^9, 
   3.5244003296869793`*^9, 3.524406307005149*^9, 3.524406577435484*^9, 
   3.524406806634446*^9, 3.524418419968019*^9, 3.5244187277574553`*^9, 
   3.524422251980219*^9, 3.5244223391402044`*^9, 3.524422778897357*^9, 
   3.524423869471734*^9, 3.5244422436216755`*^9, 3.524442769671764*^9, 
   3.5244438766880817`*^9, 3.5244457141591787`*^9, 3.5244458917423363`*^9, 
   3.524446905993886*^9, 3.524502459654175*^9, 3.5245119778979416`*^9, 
   3.5245884173153324`*^9, 3.525090305225604*^9, 3.525091712931485*^9, 
   3.52509196976824*^9, 3.5250922081188297`*^9, 3.525092463638977*^9, 
   3.525092583617162*^9, 3.525092707747345*^9, 3.525092911489631*^9, 
   3.525093231117325*^9, 3.5250987903912973`*^9, 3.525448011868248*^9, 
   3.5254612341573496`*^9, 3.5255182937570796`*^9, 3.525824603157239*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{
     FractionBox["1", "250000"], 
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"-", "685"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "-", 
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{"685", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}]}], ",", 
    RowBox[{
     FractionBox["1", "250000"], 
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"-", "514"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{"685", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}]}], ",", 
    RowBox[{
     FractionBox["1", "250000"], 
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{"685", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}]}], ",", 
    RowBox[{
     FractionBox["1", "250000"], 
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"-", "514"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
        RowBox[{"685", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}]}], ",", 
    RowBox[{"-", 
     FractionBox[
      RowBox[{"17399", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], 
      "50000"]}]}], "}"}], "}"}]], "Output",
 CellChangeTimes->{{3.5158956386727686`*^9, 3.5158956398638363`*^9}, {
   3.5158958866799536`*^9, 3.515895900344735*^9}, 3.515896802422331*^9, 
   3.515897627033496*^9, 3.5159462523356647`*^9, 3.515985921767785*^9, 
   3.516707287973062*^9, 3.5167100467628555`*^9, 3.5167114899284*^9, 
   3.516712848515107*^9, 3.5167321046199136`*^9, 3.5167517629159827`*^9, 
   3.517679445919633*^9, 3.5176799540956993`*^9, 3.517680290765956*^9, 
   3.517686202845107*^9, 3.517698252865329*^9, 3.517698806692006*^9, 
   3.520728549140354*^9, 3.5208973266942844`*^9, 3.521304022935862*^9, 
   3.521486069771743*^9, 3.521487936998542*^9, 3.52224513490092*^9, 
   3.522246821400382*^9, 3.5222499247016582`*^9, 3.522330741871931*^9, 
   3.5223566332623515`*^9, 3.522356937710122*^9, 3.5224296785949993`*^9, 
   3.5225106422336226`*^9, 3.5225114183960166`*^9, 3.522511902505706*^9, 
   3.522708167077591*^9, 3.52277517434692*^9, 3.5227768699202423`*^9, 
   3.523028613473358*^9, 3.5230313612175198`*^9, 3.523557292400724*^9, 
   3.5237248928757167`*^9, 3.5237265842264566`*^9, 3.5239837637362833`*^9, 
   3.523985011164632*^9, 3.523989325828417*^9, 3.523990349878989*^9, 
   3.523992043299847*^9, 3.5239921551712456`*^9, 3.523994836828628*^9, 
   3.523994946803918*^9, 3.523995057892272*^9, 3.524248160081889*^9, 
   3.5242485466930017`*^9, 3.5242500287246056`*^9, 3.5244001564861712`*^9, 
   3.5244003296869793`*^9, 3.524406307005149*^9, 3.524406577435484*^9, 
   3.524406806634446*^9, 3.524418419968019*^9, 3.5244187277574553`*^9, 
   3.524422251980219*^9, 3.5244223391402044`*^9, 3.524422778897357*^9, 
   3.524423869471734*^9, 3.5244422436216755`*^9, 3.524442769671764*^9, 
   3.5244438766880817`*^9, 3.5244457141591787`*^9, 3.5244458917423363`*^9, 
   3.524446905993886*^9, 3.524502459654175*^9, 3.5245119778979416`*^9, 
   3.5245884173153324`*^9, 3.525090305225604*^9, 3.525091712931485*^9, 
   3.52509196976824*^9, 3.5250922081188297`*^9, 3.525092463638977*^9, 
   3.525092583617162*^9, 3.525092707747345*^9, 3.525092911489631*^9, 
   3.525093231117325*^9, 3.5250987903912973`*^9, 3.525448011868248*^9, 
   3.5254612341573496`*^9, 3.5255182937570796`*^9, 3.5258246031602387`*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_dot_minus\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Dh", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158874776569853`*^9, 3.5158874849574027`*^9}, {
  3.515887517663273*^9, 3.5158875400105515`*^9}, {3.515887575902604*^9, 
  3.5158875781407323`*^9}, {3.515895688195601*^9, 3.515895688494618*^9}, {
  3.51589577315246*^9, 3.515895776011624*^9}, {3.515895808711494*^9, 
  3.515895808919506*^9}, {3.5158958389342227`*^9, 3.515895841410364*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Run", "[", "\"\<perl math2matopt_torso2.pl\>\"", 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<buildopt_torso\>\""}],
    "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.515897431926337*^9, {3.5158975074906588`*^9, 3.5158975156601257`*^9}, 
   3.515946262132225*^9, 3.5159464148519597`*^9, 3.5207285400618353`*^9}],

Cell[BoxData["0"], "Output",
 CellChangeTimes->{
  3.5158942533265314`*^9, 3.5158942880935197`*^9, 3.5158943288598514`*^9, 
   3.515894540344948*^9, 3.515894730479823*^9, 3.5158951421403685`*^9, {
   3.515895197692546*^9, 3.515895216536624*^9}, 3.5158957825950003`*^9, 
   3.5158958472696996`*^9, {3.5158958868709645`*^9, 3.5158959006497526`*^9}, 
   3.5158968026583443`*^9, 3.5158976273045115`*^9, 3.515899002372161*^9, 
   3.515899444381442*^9, 3.515899516217551*^9, 3.515946252575678*^9, 
   3.5159463809820223`*^9, 3.5159464201432624`*^9, 3.5159466873265443`*^9, 
   3.5159582714411182`*^9, 3.5159588013434267`*^9, 3.5159632640938406`*^9, 
   3.5159635466520014`*^9, 3.51597316587319*^9, 3.5159737776421804`*^9, 
   3.5159738712395344`*^9, 3.5159859224928265`*^9, 3.5167072881200705`*^9, 
   3.5167100469328656`*^9, 3.5167114900894094`*^9, 3.5167122547441454`*^9, 
   3.5167128486161127`*^9, 3.516732104761922*^9, 3.516751763054991*^9, 
   3.5176794461376457`*^9, 3.517679954403717*^9, 3.517680290953966*^9, 
   3.5176862030151167`*^9, 3.517698252965335*^9, 3.5176988068760166`*^9, 
   3.5207285492783623`*^9, 3.5208973270173025`*^9, 3.5213040231208725`*^9, 
   3.5214860699177513`*^9, 3.521487937090547*^9, 3.522245135005926*^9, 
   3.52224682153039*^9, 3.522249924857668*^9, 3.5223307420439415`*^9, 
   3.522356633418352*^9, 3.522356937803723*^9, 3.522429678706005*^9, 
   3.5225106423946323`*^9, 3.522511169265767*^9, 3.522511418613029*^9, 
   3.5225119026857166`*^9, 3.5227081672426004`*^9, 3.522775174405923*^9, 
   3.5227768700482492`*^9, 3.5230286136163664`*^9, 3.5230313613095255`*^9, 
   3.5235572925177307`*^9, 3.5237248929757223`*^9, 3.523726584300461*^9, 
   3.5239837637952867`*^9, 3.5239850112496367`*^9, 3.523989325897421*^9, 
   3.523990349944993*^9, 3.523992043393852*^9, 3.52399215524125*^9, 
   3.5239948369206333`*^9, 3.5239949468979235`*^9, 3.5239950579642763`*^9, 
   3.5242481601638937`*^9, 3.524248546765006*^9, 3.5242500336108847`*^9, 
   3.524400156546171*^9, 3.5244003298319883`*^9, 3.5244063070871515`*^9, 
   3.5244065775044875`*^9, 3.52440680670545*^9, 3.5244184200480194`*^9, 
   3.52441872783646*^9, 3.5244222520602236`*^9, 3.5244223391972075`*^9, 
   3.5244227789383593`*^9, 3.5244238695467386`*^9, 3.5244422436906796`*^9, 
   3.524442769744768*^9, 3.5244438767490854`*^9, 3.5244457142201824`*^9, 
   3.5244458918003397`*^9, 3.5244469061228933`*^9, 3.5245024598101835`*^9, 
   3.5245119780499506`*^9, 3.5245884173863363`*^9, 3.5250903053076086`*^9, 
   3.525091712991485*^9, 3.525091969842244*^9, 3.525092208180833*^9, 
   3.5250924637039804`*^9, 3.5250925836771655`*^9, 3.5250927078223495`*^9, 
   3.525092911592637*^9, 3.525093231243332*^9, 3.525098790547306*^9, 
   3.525448012031257*^9, 3.5254612342233534`*^9, 3.525518293902088*^9, 
   3.525824603411253*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"tf", "=", 
   RowBox[{"AbsoluteTime", "[", "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.522246710599045*^9, 3.5222467142022505`*^9}}],

Cell["Computatin time in seconds", "Text",
 CellChangeTimes->{{3.5222467382666273`*^9, 3.522246741185794*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"tf", "-", "t0"}]], "Input",
 CellChangeTimes->{{3.522246715815343*^9, 3.522246742443866*^9}}],

Cell[BoxData["136.3718`9.58626956637818"], "Output",
 CellChangeTimes->CompressedData["
1:eJwV0F0oqwEcx/HHSTlFonRORxHyMm956USM2bNhK29raFt5KTkh9diFJs8k
bVzKcfYUlraURlqLrSgXaChFS7l4KLmSw1N0lJeNk/z+F78+d7+Lb3rPkP5X
FMMwNdiB05+Uo5DYYKVrXgZt48F4HdT3FAU6oGZLfkH++CJ2TsDfQvPNCnzU
tGtW4U+HaCd10q3xa43EKjNDCbnQUr1/TXpTtk6X4ZU5X+aBKvmfZ49SYi8L
lo7DkCmPdUfg9ixfOsJKrON2IES2yXbuRqFJvjvAQ0nkqq3Q/Le/gcyyuZ1j
UEzsXiX1PuUmOZcY0xiG0WemMdItlJdFYBU/a1ep8JMnXyD3B3sn6yDjK/GQ
U+9ikIxk2LkGuFfwXyAnj+xNTVDN9RnJV2PXDBlIC3xvhopXS4IBPlkq9OTa
x/wl+S1ZGzRCk5M/J8sMdQYTzH60jgxDPqROGYXrH4VmF/znzZtwqCX2ojW9
WIDxviodGVcZw5HRJeFpMnXpZZHceLN6SUHxdEjWP9zdk/6us+U5qPbbclW1
6J6vdWnhSeEO3wulFi72DX4C70P0+A==
  "]]
}, Open  ]],

Cell["\<\
Times on various platforms (add yours in):\
\>", "Text",
 CellChangeTimes->{{3.5222468263546658`*^9, 3.522246844858724*^9}, {
  3.5222469044081297`*^9, 3.5222469067202625`*^9}}],

Cell["\<\
980x Gulftown, SSD \[Rule] 43.57 seconds\
\>", "Item",
 CellChangeTimes->{{3.522246852675171*^9, 3.522246891440388*^9}}]
}, Open  ]]
}, Open  ]]
}, Open  ]]
},
WindowSize->{1920, 1026},
WindowMargins->{{-9, Automatic}, {Automatic, -8}},
ShowSelection->True,
Magnification->1.100000023841858,
FrontEndVersion->"8.0 for Linux x86 (64-bit) (November 7, 2010)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{
 "Info3527158436-4200434"->{
  Cell[158267, 4108, 14143, 320, 933, "Input",
   CellTags->"Info3527158436-4200434"]}
 }
*)
(*CellTagsIndex
CellTagsIndex->{
 {"Info3527158436-4200434", 808442, 20171}
 }
*)
(*NotebookFileOutline
Notebook[{
Cell[CellGroupData[{
Cell[567, 22, 336, 5, 91, "Title"],
Cell[CellGroupData[{
Cell[928, 31, 196, 3, 78, "Section"],
Cell[1127, 36, 781, 17, 108, "Input"],
Cell[1911, 55, 173, 4, 31, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[2121, 64, 154, 2, 78, "Section"],
Cell[CellGroupData[{
Cell[2300, 70, 3217, 51, 51, "Input"],
Cell[5520, 123, 997, 22, 51, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[6554, 150, 127, 2, 31, "Input"],
Cell[6684, 154, 229, 5, 51, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[6950, 164, 127, 2, 31, "Input"],
Cell[7080, 168, 227, 5, 51, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[7344, 178, 752, 17, 54, "Input"],
Cell[8099, 197, 1074, 27, 51, "Output"]
}, Open  ]],
Cell[9188, 227, 198, 4, 31, "Input"],
Cell[9389, 233, 2183, 47, 51, "Input"],
Cell[11575, 282, 105, 1, 30, "Text"],
Cell[11683, 285, 105, 1, 30, "Text"],
Cell[11791, 288, 693, 19, 31, "Input"],
Cell[12487, 309, 296, 6, 30, "Text"],
Cell[12786, 317, 3859, 94, 277, "Input"],
Cell[16648, 413, 831, 24, 34, "Input"],
Cell[17482, 439, 722, 22, 34, "Input"],
Cell[CellGroupData[{
Cell[18229, 465, 135, 2, 44, "Subsection"],
Cell[18367, 469, 783, 22, 77, "Input"],
Cell[19153, 493, 641, 20, 56, "Input"],
Cell[CellGroupData[{
Cell[19819, 517, 148, 2, 31, "Subsubsection"],
Cell[19970, 521, 153, 2, 33, "Text"],
Cell[20126, 525, 5023, 123, 181, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[25186, 653, 134, 2, 31, "Subsubsection"],
Cell[CellGroupData[{
Cell[25345, 659, 5089, 124, 288, "Input"],
Cell[30437, 785, 857, 24, 117, "Output"],
Cell[31297, 811, 887, 25, 127, "Output"],
Cell[32187, 838, 908, 26, 127, "Output"],
Cell[33098, 866, 864, 24, 127, "Output"],
Cell[33965, 892, 834, 23, 117, "Output"]
}, Open  ]],
Cell[34814, 918, 1566, 47, 37, "Input"],
Cell[36383, 967, 1569, 47, 37, "Input"],
Cell[37955, 1016, 1548, 47, 37, "Input"],
Cell[39506, 1065, 1549, 47, 37, "Input"],
Cell[41058, 1114, 1548, 47, 37, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[42643, 1166, 203, 3, 31, "Subsubsection"],
Cell[42849, 1171, 7283, 203, 134, "Input"],
Cell[50135, 1376, 2721, 77, 139, "Input"],
Cell[52859, 1455, 5578, 157, 139, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[58474, 1617, 145, 2, 31, "Subsubsection"],
Cell[CellGroupData[{
Cell[58644, 1623, 2613, 69, 56, "Input"],
Cell[61260, 1694, 12964, 317, 80, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[74273, 2017, 117, 1, 31, "Subsubsection"],
Cell[CellGroupData[{
Cell[74415, 2022, 1481, 32, 68, "Input"],
Cell[75899, 2056, 2912, 81, 80, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[78860, 2143, 184, 3, 31, "Subsubsection"],
Cell[79047, 2148, 510, 13, 68, "Input"],
Cell[79560, 2163, 219, 4, 33, "Text"],
Cell[CellGroupData[{
Cell[79804, 2171, 637, 13, 34, "Input"],
Cell[80444, 2186, 45353, 1138, 182, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[125846, 3330, 142, 2, 31, "Subsubsection"],
Cell[125991, 3334, 877, 16, 34, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[126905, 3355, 253, 5, 31, "Subsubsection"],
Cell[127161, 3362, 1153, 30, 94, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[128351, 3397, 308, 6, 31, "Subsubsection"],
Cell[128662, 3405, 204, 3, 33, "Text"],
Cell[128869, 3410, 439, 11, 34, "Input"],
Cell[129311, 3423, 1724, 49, 34, "Input"],
Cell[131038, 3474, 954, 23, 39, "Input"],
Cell[CellGroupData[{
Cell[132017, 3501, 1073, 31, 34, "Input"],
Cell[133093, 3534, 3109, 55, 56, "Output"]
}, Open  ]],
Cell[136217, 3592, 751, 21, 57, "Input"],
Cell[136971, 3615, 447, 12, 97, "Input"],
Cell[137421, 3629, 702, 16, 97, "Input"],
Cell[138126, 3647, 702, 16, 97, "Input"],
Cell[138831, 3665, 777, 17, 97, "Input"],
Cell[139611, 3684, 799, 17, 97, "Input"],
Cell[140413, 3703, 1031, 21, 97, "Input"],
Cell[141447, 3726, 1013, 20, 97, "Input"],
Cell[142463, 3748, 1077, 22, 97, "Input"]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[143601, 3777, 158, 2, 85, "Section"],
Cell[CellGroupData[{
Cell[143784, 3783, 101, 1, 44, "Subsection"],
Cell[CellGroupData[{
Cell[143910, 3788, 121, 1, 31, "Subsubsection"],
Cell[CellGroupData[{
Cell[144056, 3793, 236, 5, 34, "Input"],
Cell[144295, 3800, 2519, 38, 34, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[146863, 3844, 82, 1, 31, "Subsubsection"],
Cell[CellGroupData[{
Cell[146970, 3849, 493, 11, 34, "Input"],
Cell[147466, 3862, 2759, 44, 34, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[150274, 3912, 115, 1, 31, "Subsubsection"],
Cell[CellGroupData[{
Cell[150414, 3917, 1018, 27, 62, "Input"],
Cell[151435, 3946, 6817, 159, 81, "Output"]
}, Open  ]],
Cell[158267, 4108, 14143, 320, 933, "Input",
 CellTags->"Info3527158436-4200434"]
}, Open  ]],
Cell[CellGroupData[{
Cell[172447, 4433, 130, 1, 31, "Subsubsection"],
Cell[172580, 4436, 2164, 60, 62, "Input"],
Cell[CellGroupData[{
Cell[174769, 4500, 4059, 126, 118, "Input"],
Cell[178831, 4628, 2532, 39, 34, "Output"],
Cell[181366, 4669, 2532, 39, 34, "Output"],
Cell[183901, 4710, 2532, 39, 34, "Output"],
Cell[186436, 4751, 2513, 38, 34, "Output"],
Cell[188952, 4791, 2566, 40, 56, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[191555, 4836, 323, 6, 34, "Input"],
Cell[191881, 4844, 2881, 50, 56, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[194811, 4900, 111, 1, 31, "Subsubsection"],
Cell[CellGroupData[{
Cell[194947, 4905, 625, 15, 62, "Input"],
Cell[195575, 4922, 3197, 61, 59, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[198821, 4989, 122, 1, 31, "Subsubsection"],
Cell[CellGroupData[{
Cell[198968, 4994, 1523, 45, 62, "Input"],
Cell[200494, 5041, 2655, 44, 56, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[203186, 5090, 3857, 125, 118, "Input"],
Cell[207046, 5217, 2530, 39, 34, "Output"],
Cell[209579, 5258, 2586, 41, 56, "Output"],
Cell[212168, 5301, 2449, 35, 34, "Output"],
Cell[214620, 5338, 2449, 35, 34, "Output"],
Cell[217072, 5375, 2451, 35, 34, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[219560, 5415, 275, 6, 34, "Input"],
Cell[219838, 5423, 2675, 44, 56, "Output"]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[222574, 5474, 151, 2, 44, "Subsection"],
Cell[CellGroupData[{
Cell[222750, 5480, 109, 1, 31, "Subsubsection"],
Cell[CellGroupData[{
Cell[222884, 5485, 1020, 29, 62, "Input"],
Cell[223907, 5516, 6428, 150, 81, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[230384, 5672, 120, 1, 31, "Subsubsection"],
Cell[CellGroupData[{
Cell[230529, 5677, 2030, 58, 62, "Input"],
Cell[232562, 5737, 5064, 115, 56, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[237663, 5857, 3771, 122, 118, "Input"],
Cell[241437, 5981, 2430, 38, 34, "Output"],
Cell[243870, 6021, 2430, 38, 34, "Output"],
Cell[246303, 6061, 2430, 38, 34, "Output"],
Cell[248736, 6101, 2409, 37, 34, "Output"],
Cell[251148, 6140, 2466, 39, 56, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[253651, 6184, 252, 5, 34, "Input"],
Cell[253906, 6191, 2815, 49, 56, "Output"]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[256782, 6247, 96, 1, 44, "Subsection"],
Cell[CellGroupData[{
Cell[256903, 6252, 99, 1, 31, "Subsubsection"],
Cell[CellGroupData[{
Cell[257027, 6257, 276, 7, 34, "Input"],
Cell[257306, 6266, 2467, 39, 34, "Output"]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[259834, 6312, 102, 1, 44, "Subsection"],
Cell[CellGroupData[{
Cell[259961, 6317, 103, 1, 31, "Subsubsection"],
Cell[260067, 6320, 348, 10, 34, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[260452, 6335, 113, 1, 31, "Subsubsection"],
Cell[260568, 6338, 522, 17, 34, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[261127, 6360, 128, 1, 31, "Subsubsection"],
Cell[261258, 6363, 497, 15, 34, "Input"],
Cell[261758, 6380, 68, 1, 34, "Input"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[261875, 6387, 91, 1, 44, "Subsection"],
Cell[CellGroupData[{
Cell[261991, 6392, 242, 5, 34, "Input"],
Cell[262236, 6399, 1435, 42, 56, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[263708, 6446, 1769, 38, 68, "Input"],
Cell[265480, 6486, 3200, 85, 80, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[268717, 6576, 721, 20, 34, "Input"],
Cell[269441, 6598, 965, 25, 56, "Output"]
}, Open  ]],
Cell[270421, 6626, 92, 1, 34, "Input"],
Cell[CellGroupData[{
Cell[270538, 6631, 28, 0, 34, "Input"],
Cell[270569, 6633, 797, 23, 36, "Output"]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[271427, 6663, 108, 1, 85, "Section"],
Cell[271538, 6666, 601, 16, 97, "Input"],
Cell[272142, 6684, 588, 16, 97, "Input"],
Cell[272733, 6702, 662, 17, 97, "Input"],
Cell[273398, 6721, 583, 16, 97, "Input"],
Cell[273984, 6739, 577, 16, 97, "Input"],
Cell[274564, 6757, 579, 16, 97, "Input"],
Cell[275146, 6775, 1131, 32, 202, "Input"],
Cell[276280, 6809, 1166, 35, 202, "Input"],
Cell[CellGroupData[{
Cell[277471, 6848, 91, 1, 44, "Subsection"],
Cell[277565, 6851, 130, 1, 33, "Text"],
Cell[CellGroupData[{
Cell[277720, 6856, 1269, 23, 77, "Input"],
Cell[278992, 6881, 3755, 53, 34, "Output"]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[282808, 6941, 101, 1, 85, "Section"],
Cell[282912, 6944, 185, 2, 33, "Text"],
Cell[283100, 6948, 475, 11, 56, "Input"],
Cell[CellGroupData[{
Cell[283600, 6963, 720, 20, 57, "Input"],
Cell[284323, 6985, 3021, 52, 52, "Output"],
Cell[287347, 7039, 3415, 64, 51, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[290799, 7108, 1683, 38, 101, "Input"],
Cell[292485, 7148, 2735, 48, 52, "Output"],
Cell[295223, 7198, 3129, 60, 51, "Output"],
Cell[298355, 7260, 3246, 62, 52, "Output"]
}, Open  ]],
Cell[301616, 7325, 163, 2, 33, "Text"],
Cell[CellGroupData[{
Cell[301804, 7331, 1016, 20, 64, "Input"],
Cell[302823, 7353, 1572, 37, 67, "Output"]
}, Open  ]],
Cell[304410, 7393, 590, 16, 49, "Input"],
Cell[CellGroupData[{
Cell[305025, 7413, 261, 5, 34, "Input"],
Cell[305289, 7420, 4006, 69, 67, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[309332, 7494, 263, 8, 31, "Subsubsection"],
Cell[309598, 7504, 986, 30, 63, "Input"],
Cell[CellGroupData[{
Cell[310609, 7538, 860, 22, 56, "Input"],
Cell[311472, 7562, 3325, 47, 34, "Output"]
}, Open  ]],
Cell[314812, 7612, 1243, 33, 58, "Input"],
Cell[316058, 7647, 1049, 33, 58, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[317144, 7685, 189, 2, 31, "Subsubsection"],
Cell[CellGroupData[{
Cell[317358, 7691, 1428, 22, 34, "Input"],
Cell[318789, 7715, 1779, 45, 52, "Output"]
}, Open  ]],
Cell[320583, 7763, 827, 18, 56, "Input"],
Cell[CellGroupData[{
Cell[321435, 7785, 1068, 27, 34, "Input"],
Cell[322506, 7814, 5462, 114, 56, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[328005, 7933, 1049, 29, 58, "Input"],
Cell[329057, 7964, 3776, 100, 52, "Output"],
Cell[332836, 8066, 8470, 225, 134, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[341343, 8296, 1426, 37, 58, "Input"],
Cell[342772, 8335, 8120, 217, 132, "Output"],
Cell[350895, 8554, 40363, 1038, 445, "Output"]
}, Open  ]],
Cell[391273, 9595, 26, 0, 34, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[391336, 9600, 122, 1, 31, "Subsubsection"],
Cell[391461, 9603, 884, 19, 97, "Input"],
Cell[392348, 9624, 903, 20, 97, "Input"],
Cell[393254, 9646, 624, 16, 99, "Input"],
Cell[393881, 9664, 962, 21, 97, "Input"],
Cell[394846, 9687, 1052, 23, 99, "Input"],
Cell[395901, 9712, 915, 20, 97, "Input"],
Cell[396819, 9734, 637, 17, 97, "Input"],
Cell[397459, 9753, 834, 20, 97, "Input"],
Cell[398296, 9775, 642, 17, 99, "Input"],
Cell[398941, 9794, 783, 19, 99, "Input"],
Cell[399727, 9815, 692, 18, 99, "Input"],
Cell[400422, 9835, 696, 18, 99, "Input"],
Cell[401121, 9855, 637, 17, 97, "Input"],
Cell[401761, 9874, 740, 19, 97, "Input"],
Cell[402504, 9895, 639, 17, 99, "Input"],
Cell[403146, 9914, 741, 19, 99, "Input"],
Cell[403890, 9935, 812, 20, 99, "Input"],
Cell[404705, 9957, 642, 17, 99, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[405384, 9979, 91, 1, 44, "Subsection"],
Cell[405478, 9982, 130, 1, 33, "Text"],
Cell[CellGroupData[{
Cell[405633, 9987, 1220, 23, 77, "Input"],
Cell[406856, 10012, 3679, 51, 34, "Output"]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[410596, 10070, 149, 2, 85, "Section"],
Cell[410748, 10074, 174, 5, 45, "Input"],
Cell[CellGroupData[{
Cell[410947, 10083, 546, 16, 75, "Input"],
Cell[411496, 10101, 2536, 67, 80, "Output"],
Cell[414035, 10170, 1135, 25, 45, "Output"]
}, Open  ]],
Cell[415185, 10198, 897, 23, 75, "Input"],
Cell[416085, 10223, 531, 13, 45, "Input"],
Cell[416619, 10238, 505, 14, 135, "Input"],
Cell[417127, 10254, 410, 12, 135, "Input"],
Cell[417540, 10268, 505, 13, 135, "Input"],
Cell[CellGroupData[{
Cell[418070, 10285, 937, 26, 75, "Input"],
Cell[419010, 10313, 863, 15, 45, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[419910, 10333, 275, 5, 79, "Input"],
Cell[420188, 10340, 1203, 28, 45, "Output"],
Cell[421394, 10370, 1854, 49, 67, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[423285, 10424, 1088, 31, 108, "Input"],
Cell[424376, 10457, 3047, 55, 110, "Output"]
}, Open  ]],
Cell[427438, 10515, 1066, 31, 115, "Input"],
Cell[428507, 10548, 506, 15, 48, "Input"],
Cell[429016, 10565, 692, 17, 48, "Input"],
Cell[429711, 10584, 796, 23, 48, "Input"],
Cell[430510, 10609, 467, 14, 45, "Input"],
Cell[CellGroupData[{
Cell[431002, 10627, 149, 2, 45, "Input"],
Cell[431154, 10631, 16101, 456, 952, "Output"]
}, Open  ]],
Cell[447270, 11090, 491, 14, 148, "Input"],
Cell[447764, 11106, 470, 13, 148, "Input"],
Cell[448237, 11121, 112, 3, 49, "Input"],
Cell[448352, 11126, 823, 22, 52, "Input"],
Cell[449178, 11150, 451, 12, 148, "Input"],
Cell[CellGroupData[{
Cell[449654, 11166, 200, 5, 49, "Input"],
Cell[449857, 11173, 2643, 45, 74, "Output"]
}, Open  ]],
Cell[452515, 11221, 511, 15, 49, "Input"],
Cell[CellGroupData[{
Cell[453051, 11240, 310, 9, 49, "Input"],
Cell[453364, 11251, 4439, 112, 276, "Output"]
}, Open  ]],
Cell[457818, 11366, 491, 14, 148, "Input"],
Cell[CellGroupData[{
Cell[458334, 11384, 658, 15, 115, "Input"],
Cell[458995, 11401, 2585, 43, 73, "Output"],
Cell[461583, 11446, 20449, 503, 1642, "Output"]
}, Open  ]],
Cell[482047, 11952, 456, 12, 148, "Input"],
Cell[CellGroupData[{
Cell[482528, 11968, 523, 14, 115, "Input"],
Cell[483054, 11984, 1696, 45, 74, "Output"]
}, Open  ]],
Cell[484765, 12032, 460, 12, 148, "Input"],
Cell[CellGroupData[{
Cell[485250, 12048, 674, 19, 82, "Input"],
Cell[485927, 12069, 2380, 37, 73, "Output"],
Cell[488310, 12108, 2376, 37, 73, "Output"]
}, Open  ]],
Cell[490701, 12148, 450, 12, 148, "Input"],
Cell[CellGroupData[{
Cell[491176, 12164, 200, 4, 49, "Input"],
Cell[491379, 12170, 31917, 818, 2096, "Output"]
}, Open  ]],
Cell[523311, 12991, 451, 12, 148, "Input"],
Cell[CellGroupData[{
Cell[523787, 13007, 540, 14, 115, "Input"],
Cell[524330, 13023, 2415, 33, 49, "Output"]
}, Open  ]]
}, Closed]],
Cell[CellGroupData[{
Cell[526794, 13062, 251, 3, 49, "Section"],
Cell[527048, 13067, 270, 7, 34, "Input"],
Cell[527321, 13076, 102, 1, 33, "Text"],
Cell[527426, 13079, 1163, 26, 118, "Input"],
Cell[528592, 13107, 238, 3, 33, "Text"],
Cell[528833, 13112, 302, 7, 34, "Input"],
Cell[529138, 13121, 174, 2, 33, "Text"],
Cell[CellGroupData[{
Cell[529337, 13127, 796, 22, 97, "Input"],
Cell[530136, 13151, 3813, 73, 34, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[533986, 13229, 184, 2, 44, "Subsection"],
Cell[CellGroupData[{
Cell[534195, 13235, 1098, 26, 58, "Input"],
Cell[535296, 13263, 3458, 59, 52, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[538791, 13327, 34, 0, 34, "Input"],
Cell[538828, 13329, 1949, 50, 34, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[540814, 13384, 186, 3, 34, "Input"],
Cell[541003, 13389, 1962, 48, 51, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[543002, 13442, 110, 3, 34, "Input"],
Cell[543115, 13447, 1427, 36, 67, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[544579, 13488, 687, 16, 56, "Input"],
Cell[545269, 13506, 4317, 85, 55, "Output"]
}, Open  ]],
Cell[549601, 13594, 444, 12, 97, "Input"],
Cell[550048, 13608, 137, 1, 33, "Text"],
Cell[CellGroupData[{
Cell[550210, 13613, 465, 13, 34, "Input"],
Cell[550678, 13628, 26940, 695, 466, "Output"]
}, Open  ]],
Cell[577633, 14326, 136, 1, 42, "Text"],
Cell[CellGroupData[{
Cell[577794, 14331, 301, 8, 45, "Input"],
Cell[578098, 14341, 4344, 98, 140, "Output"]
}, Open  ]],
Cell[582457, 14442, 514, 14, 163, "Input"],
Cell[CellGroupData[{
Cell[582996, 14460, 274, 6, 45, "Input"],
Cell[583273, 14468, 35414, 866, 1743, "Output"]
}, Open  ]],
Cell[618702, 15337, 477, 13, 163, "Input"],
Cell[CellGroupData[{
Cell[619204, 15354, 327, 4, 39, "Subsubsection"],
Cell[619534, 15360, 131, 1, 42, "Text"],
Cell[619668, 15363, 2315, 59, 399, "Input"],
Cell[621986, 15424, 3384, 99, 339, "Input"],
Cell[625373, 15525, 418, 12, 45, "Input"],
Cell[CellGroupData[{
Cell[625816, 15541, 124, 2, 45, "Input"],
Cell[625943, 15545, 1895, 44, 139, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[627875, 15594, 716, 19, 75, "Input"],
Cell[628594, 15615, 4919, 132, 187, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[633550, 15752, 336, 7, 45, "Input"],
Cell[633889, 15761, 6598, 151, 187, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[640524, 15917, 4126, 104, 251, "Input"],
Cell[644653, 16023, 2839, 42, 45, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[647529, 16070, 1301, 34, 163, "Input"],
Cell[648833, 16106, 5544, 149, 259, "Output"],
Cell[654380, 16257, 934, 20, 45, "Output"],
Cell[655317, 16279, 1070, 24, 45, "Output"],
Cell[656390, 16305, 8467, 225, 500, "Output"],
Cell[664860, 16532, 934, 20, 45, "Output"]
}, Open  ]],
Cell[665809, 16555, 608, 15, 135, "Input"],
Cell[666420, 16572, 632, 15, 135, "Input"],
Cell[667055, 16589, 632, 15, 135, "Input"],
Cell[667690, 16606, 634, 15, 135, "Input"],
Cell[668327, 16623, 634, 15, 135, "Input"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[669010, 16644, 331, 4, 57, "Subsection"],
Cell[669344, 16650, 126, 1, 42, "Text"],
Cell[669473, 16653, 850, 23, 45, "Input"],
Cell[CellGroupData[{
Cell[670348, 16680, 301, 7, 45, "Input"],
Cell[670652, 16689, 20751, 555, 1218, "Output"]
}, Open  ]],
Cell[691418, 17247, 489, 14, 135, "Input"],
Cell[CellGroupData[{
Cell[691932, 17265, 237, 5, 45, "Input"],
Cell[692172, 17272, 97908, 2467, 3771, "Output"]
}, Open  ]],
Cell[790095, 19742, 451, 12, 135, "Input"],
Cell[CellGroupData[{
Cell[790571, 19758, 375, 9, 75, "Input"],
Cell[790949, 19769, 3555, 71, 127, "Output"],
Cell[794507, 19842, 7871, 190, 425, "Output"]
}, Open  ]],
Cell[802393, 20035, 803, 18, 135, "Input"],
Cell[CellGroupData[{
Cell[803221, 20057, 565, 14, 105, "Input"],
Cell[803789, 20073, 2800, 39, 45, "Output"]
}, Open  ]],
Cell[806604, 20115, 171, 4, 45, "Input"],
Cell[806778, 20121, 110, 1, 42, "Text"],
Cell[CellGroupData[{
Cell[806913, 20126, 117, 2, 45, "Input"],
Cell[807033, 20130, 677, 12, 45, "Output"]
}, Open  ]],
Cell[807725, 20145, 187, 4, 42, "Text"],
Cell[807915, 20151, 130, 3, 31, "Item"]
}, Open  ]]
}, Open  ]]
}, Open  ]]
}
]
*)

(* End of internal cache information *)
